From 283996437588c7cd0d6759094758c773437cf5c5 Mon Sep 17 00:00:00 2001 From: Prashanth Ramadoss Date: Fri, 7 Sep 2018 16:14:42 +0200 Subject: [PATCH 01/25] [virtAnalogWrapper] Adding virtualAnalogWrapper impl to controlboard Controlboard must read the configuration parameters for opening the virtual analog wrapper device. It must be noted that few parameters fall under the group VIRTUAL_ANALOG_WRAPPER group, few parameters are borrowed from the WRAPPER group and useVirtualAnalogSensor parameter belongs to top level. The virtual analog wrapper device attaches itself to the controlboard which implements the methods for the IVirtualAnalogSensor interface. --- plugins/controlboard/CMakeLists.txt | 3 +- .../include/gazebo/ControlBoard.hh | 5 ++ .../include/yarp/dev/ControlBoardDriver.h | 17 ++++- plugins/controlboard/src/ControlBoard.cc | 62 +++++++++++++++-- .../controlboard/src/ControlBoardDriver.cpp | 12 +++- .../ControlBoardDriverVirtualAnalogSensor.cpp | 68 +++++++++++++++++++ 6 files changed, 158 insertions(+), 9 deletions(-) create mode 100644 plugins/controlboard/src/ControlBoardDriverVirtualAnalogSensor.cpp diff --git a/plugins/controlboard/CMakeLists.txt b/plugins/controlboard/CMakeLists.txt index 7bc802595..f9a1d661c 100644 --- a/plugins/controlboard/CMakeLists.txt +++ b/plugins/controlboard/CMakeLists.txt @@ -36,7 +36,8 @@ set(controlBoard_source src/ControlBoard.cc src/ControlBoardDriverCurrent.cpp src/ControlBoardDriverPidControl.cpp src/ControlBoardDriverTrajectory.cpp - src/ControlBoardDriverCoupling.cpp) + src/ControlBoardDriverCoupling.cpp + src/ControlBoardDriverVirtualAnalogSensor.cpp) set(controlBoard_headers include/gazebo/ControlBoard.hh include/yarp/dev/ControlBoardDriver.h diff --git a/plugins/controlboard/include/gazebo/ControlBoard.hh b/plugins/controlboard/include/gazebo/ControlBoard.hh index e2d1c265e..90686fa45 100644 --- a/plugins/controlboard/include/gazebo/ControlBoard.hh +++ b/plugins/controlboard/include/gazebo/ControlBoard.hh @@ -12,6 +12,7 @@ #include #include +#include namespace yarp { namespace dev { @@ -51,6 +52,10 @@ private: yarp::dev::IMultipleWrapper* m_iWrap; yarp::dev::PolyDriverList m_controlBoards; + bool m_useVirtAnalogSensor = false; + yarp::dev::PolyDriver m_virtAnalogSensorWrapper; + yarp::dev::IMultipleWrapper* m_iVirtAnalogSensorWrap; + yarp::os::Property m_parameters; std::string m_robotName; diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h index 2906851dd..23b49ca82 100644 --- a/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h +++ b/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h @@ -8,6 +8,7 @@ #define GAZEBOYARP_CONTROLBOARDDRIVER_HH #include +#include #include #include #include @@ -18,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -94,8 +96,9 @@ class yarp::dev::GazeboYarpControlBoardDriver: public ICurrentControl, public IPidControl, public IRemoteVariables, - public IAxisInfo -{ + public IAxisInfo, + public IVirtualAnalogSensor + { public: GazeboYarpControlBoardDriver(); @@ -252,6 +255,12 @@ class yarp::dev::GazeboYarpControlBoardDriver: virtual bool setRefCurrents(const int n_joint, const int *joints, const double *t) override; virtual bool getRefCurrents(double *t) override; virtual bool getRefCurrent(int j, double *t) override; + + // Virtual Analog Sensor Interface + virtual VAS_status getVirtualAnalogSensorStatus(int ch) override; + virtual int getVirtualAnalogSensorChannels() override; + virtual bool updateVirtualAnalogSensorMeasure(yarp::sig::Vector &measure) override; + virtual bool updateVirtualAnalogSensorMeasure(int ch, double &measure) override; /* * IPidControl Interface methods @@ -379,6 +388,7 @@ class yarp::dev::GazeboYarpControlBoardDriver: yarp::sig::Vector m_motPositions; /**< motor positions [Degrees] */ yarp::sig::Vector m_velocities; /**< joint velocities [Degrees/Seconds] */ yarp::sig::Vector m_torques; /**< joint torques [Netwon Meters] */ + yarp::sig::Vector m_measTorques; /**< joint torques from virtual analog sensor [Newton Meters] */ yarp::sig::Vector m_maxTorques; /**< joint torques [Netwon Meters] */ yarp::os::Stamp m_lastTimestamp; /**< timestamp, updated with simulation time at each onUpdate call */ @@ -441,7 +451,8 @@ class yarp::dev::GazeboYarpControlBoardDriver: bool* m_isMotionDone; int * m_controlMode; int * m_interactionMode; - + + bool m_useVirtualAnalogSensor = false; bool m_started; int m_clock; int _T_controller; diff --git a/plugins/controlboard/src/ControlBoard.cc b/plugins/controlboard/src/ControlBoard.cc index 16fb6b5ee..90186a9ac 100644 --- a/plugins/controlboard/src/ControlBoard.cc +++ b/plugins/controlboard/src/ControlBoard.cc @@ -73,6 +73,8 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) yarp::os::Bottle wrapper_group; yarp::os::Bottle driver_group; + yarp::os::Bottle virt_group; + if (_sdf->HasElement("yarpConfigurationFile")) { std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); @@ -85,7 +87,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) m_parameters.put("gazebo_ini_file_path",ini_file_path.c_str()); wrapper_group = m_parameters.findGroup("WRAPPER"); - if(wrapper_group.isNull()) { + if (wrapper_group.isNull()) { yError("GazeboYarpControlBoard : [WRAPPER] group not found in config file\n"); return; } @@ -96,7 +98,18 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); wrapper_group.append(yarp::os::Bottle(ROS)); } - + + m_useVirtAnalogSensor = m_parameters.check("useVirtualAnalogSensor", yarp::os::Value(false)).asBool(); + if (m_useVirtAnalogSensor) + { + virt_group = m_parameters.findGroup("VIRTUAL_ANALOG_SERVER"); + if (virt_group.isNull()) + { + yError("GazeboYarpControlBoard : [VIRTUAL_ANALOG_SERVER] group not found in config file\n"); + return; + } + } + configuration_loaded = true; } @@ -127,15 +140,32 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) m_wrapper.close(); return; } - + + if (m_useVirtAnalogSensor) + { + yarp::os::Bottle& robotName_config = virt_group.addList(); + robotName_config.addString("robotName"); + robotName_config.addString(m_robotName.c_str()); + + std::string networks = std::string("(") + wrapper_group.findGroup("networks").toString() + std::string(")"); + virt_group.append(yarp::os::Bottle(networks)); + } + for (int n = 0; n < netList->size(); n++) { yarp::dev::PolyDriverDescriptor newPoly; newPoly.key = netList->get(n).asString(); + + // initially deal with virtual analog stuff + if (m_useVirtAnalogSensor) + { + std::string net = std::string("(") + wrapper_group.findGroup(newPoly.key.c_str()).toString() + std::string(")"); + virt_group.append(yarp::os::Bottle(net)); + } + std::string scopedDeviceName = m_robotName + "::" + newPoly.key.c_str(); newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName); - if( newPoly.poly != NULL) { // device already exists, use it, setting it againg to increment the usage counter. @@ -176,11 +206,35 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly); m_controlBoards.push(newPoly); } + + if (m_useVirtAnalogSensor) + { + m_virtAnalogSensorWrapper.open(virt_group); + if (!m_virtAnalogSensorWrapper.isValid()) + { + yError("GazeboYarpControlBoard : Virtual analog sensor wrapper did not open, load failed."); + m_virtAnalogSensorWrapper.close(); + return; + } + + if (!m_virtAnalogSensorWrapper.view(m_iVirtAnalogSensorWrap)) + { + yError("GazeboYarpControlBoard : Could not view the IVirtualAnalogSensor interface"); + return; + } + + m_iVirtAnalogSensorWrap->attachAll(m_controlBoards); + } + + + + // !m_iVirtAnalogSensorWrap || !m_iVirtAnalogSensorWrap->attachAll(m_controlBoards) if (!m_iWrap || !m_iWrap->attachAll(m_controlBoards)) { yError("GazeboYarpControlBoard : error while attaching wrapper to device."); m_wrapper.close(); + m_virtAnalogSensorWrapper.close(); for (int n = 0; n < netList->size(); n++) { std::string scopedDeviceName = m_robotName + "::" + m_controlBoards[n]->key.c_str(); GazeboYarpPlugins::Handler::getHandler()->removeDevice(scopedDeviceName); diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 176821c4d..e3109db60 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -64,6 +64,7 @@ bool GazeboYarpControlBoardDriver::gazebo_init() m_velocities.resize(m_numberOfJoints); m_amp.resize(m_numberOfJoints); m_torques.resize(m_numberOfJoints); m_torques.zero(); + m_measTorques.resize(m_numberOfJoints); m_measTorques.zero(); m_maxTorques.resize(m_numberOfJoints, 2000.0); m_trajectoryGenerationReferenceSpeed.resize(m_numberOfJoints); m_jntReferencePositions.resize(m_numberOfJoints); @@ -123,6 +124,8 @@ bool GazeboYarpControlBoardDriver::gazebo_init() m_speed_ramp_handler.resize(m_numberOfJoints, NULL); m_velocity_watchdog.resize(m_numberOfJoints, NULL); + m_useVirtualAnalogSensor = m_pluginParameters.check("useVirtualAnalogSensor", yarp::os::Value(false)).asBool(); + VectorOf trajectory_generator_type; trajectory_generator_type.resize(m_numberOfJoints); @@ -466,7 +469,14 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i #endif m_positions[jnt_cnt] = convertGazeboToUser(jnt_cnt, gazeboPos); m_velocities[jnt_cnt] = convertGazeboToUser(jnt_cnt, m_jointPointers[jnt_cnt]->GetVelocity(0)); - m_torques[jnt_cnt] = m_jointPointers[jnt_cnt]->GetForce(0u); + if (!m_useVirtualAnalogSensor) + { + m_torques[jnt_cnt] = m_jointPointers[jnt_cnt]->GetForce(0u); + } + else + { + m_torques[jnt_cnt] = m_measTorques[jnt_cnt]; + } } m_motPositions=m_positions; diff --git a/plugins/controlboard/src/ControlBoardDriverVirtualAnalogSensor.cpp b/plugins/controlboard/src/ControlBoardDriverVirtualAnalogSensor.cpp new file mode 100644 index 000000000..05769fd0d --- /dev/null +++ b/plugins/controlboard/src/ControlBoardDriverVirtualAnalogSensor.cpp @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR + * Authors: see AUTHORS file. + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +#include "ControlBoardDriver.h" +#include + +namespace yarp { + namespace dev { + + + VAS_status GazeboYarpControlBoardDriver::getVirtualAnalogSensorStatus(int ch) + { + if (ch < 0 || ch >= this->getVirtualAnalogSensorChannels()) + { + yError() << "GazeboYarpControlBoardDriver: VirtualAnalogServer: getState failed: requested channel " << ch << + "while the client is configured wtih " << this->getVirtualAnalogSensorChannels() << "channels"; + + return VAS_status::VAS_ERROR; + } + + return VAS_status::VAS_OK; + } + + int GazeboYarpControlBoardDriver::getVirtualAnalogSensorChannels() + { + return m_numberOfJoints; + } + + bool GazeboYarpControlBoardDriver::updateVirtualAnalogSensorMeasure(yarp::sig::Vector &measure) + { + if (m_useVirtualAnalogSensor) + { + if (measure.size() != m_numberOfJoints) + { + yError() << "GazeboYarpControlBoardDriver: VirtualAnalogServer: assert size error : Vector lengths do not match"; + return false; + } + + for (size_t j = 0; j < m_numberOfJoints; j++) + { + this->updateVirtualAnalogSensorMeasure(j, measure[j]); + } + return true; + } + return false; + } + + bool GazeboYarpControlBoardDriver::updateVirtualAnalogSensorMeasure(int ch, double &measure) + { + if (m_useVirtualAnalogSensor) + { + if (ch < 0 || ch >= this->getVirtualAnalogSensorChannels()) + { + yError() << "GazeboYarpControlBoardDriver: VirtualAnalogServer: index out of bounds"; + return false; + } + + m_measTorques[ch] = measure; + return true; + } + return false; + } + + } +} From 0b247db47a624c1a8eada30348b6e07bcc8aa762 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Thu, 25 Oct 2018 17:56:13 +0200 Subject: [PATCH 02/25] Refactoring of linkattacher --- plugins/linkattacher/README.md | 12 +- .../linkattacherserverimpl.h | 16 +- .../src/linkattacherserverimpl.cpp | 193 +++++++++--------- 3 files changed, 115 insertions(+), 106 deletions(-) diff --git a/plugins/linkattacher/README.md b/plugins/linkattacher/README.md index b33fb8062..a8bc2658c 100644 --- a/plugins/linkattacher/README.md +++ b/plugins/linkattacher/README.md @@ -1,7 +1,8 @@ ### LinkAttacher Plugin -The LinkAttacher is a Gazebo _model plugin_. It can be used to attach/detach any _link_ of the _model_ spawned in Gazebo environment. The attachment is made by adding a _fixed joint_ between the links specified. The link of the model in Gazebo environment is the parent link and the link of the robot(to which this plugin) is used becomes the child link for the newly created fixed joint. So, the fixed joint is named by the `model_link_name` suffixed with the phrase *_magnet_joint* in the name e.g, l_hand_magnet_joint. +The LinkAttacher is a Gazebo _model plugin_. It can be used to attach/detach two _link_ of _model_ spawned in Gazebo environment. The attachment is made by adding a _fixed joint_ between the links specified. The link of the model we want to attach is the parent link and the link of the robot model (to which this plugin is added) becomes the child link for the newly created fixed joint. So, the fixed joint is named by the `model_link_name` suffixed with the phrase *_magnet_joint* in the name e.g, l_hand_magnet_joint. -Additionally, this plugin also provides control over _gravity_ of the models spawned in Gazebo environment. Currently, this plugin is used for coupling the hands of the robot model and human model in pHRI experiments. Also, this plugin can be used to attach objects to the robot links. +Additionally, this plugin also provides control over _gravity_ of the models spawned in Gazebo environment. +Currently, this plugin is used for coupling the hands of two robots, or those robot model and human model in pHRI experiments. Also, this plugin can be used to attach objects to the robot links. ### Usage Create a `linkattacher.ini` configuration file and add the following lines to it to open the rpc port for communicating with the linkattacher plugin @@ -20,6 +21,7 @@ Now, add the following lines to the sdf model On launching the model in Gazebo, the _rpc_ port will be opened. Connect to this port and type `help` to know the available commands. The available commands are: -- attachUnscoped -- detachUnscoped -- enableGravity +- `attachUnscoped ` +- `detachUnscoped ` +- `enableGravity ` +The `link_name` can be both the scoped or unscoped name of the link. \ No newline at end of file diff --git a/plugins/linkattacher/include/GazeboYarpPlugins/linkattacherserverimpl.h b/plugins/linkattacher/include/GazeboYarpPlugins/linkattacherserverimpl.h index fd2d32079..6b856912e 100644 --- a/plugins/linkattacher/include/GazeboYarpPlugins/linkattacherserverimpl.h +++ b/plugins/linkattacher/include/GazeboYarpPlugins/linkattacherserverimpl.h @@ -39,25 +39,25 @@ class LinkAttacherServerImpl: public GazeboYarpPlugins::LinkAttacherServer /** * Attach any link of the models spawned in gazebo to a link of the robot using a fixed joint. - * @param model_name name that identifies model in gazebo (that are already spawned in gazebo) - * @param model_link_name name of a the link in the model you want to attach to the robot - * @param robot_name name of the robot - * @param robot_link_name name of the robot link to which you want to attached the model link + * @param parent_model_name name that identifies the first model + * @param parent_model_link_name name of the link in the first model you want to attach (scoped or unscoped name) + * @param child_model_name name that identifies the second model + * @param child_model_link_name name of the link in the second model you want to attach (scoped or unscoped name) * @return true if success, false otherwise */ - virtual bool attachUnscoped(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name); + virtual bool attachUnscoped(const std::string& parent_model_name, const std::string& parent_model_link_name, const std::string& child_model_name, const std::string& child_model_link_name); /** * Detach the model link which was previously attached to the robot link through a fixed joint. - * @param model_name name that identifies model in gazebo (that are already spawned in gazebo) - * @param model_link_name name of a the link in the model that is attached to the robot + * @param model_name name that identifies parent model + * @param model_link_name name of the link of the parent model that is attached * @return true if success, false otherwise */ virtual bool detachUnscoped(const std::string& model_name, const std::string& model_link_name); /** * Enable/disables gravity for a model - * @param model_name name that identifies model in gazebo (that are already spawned in gazebo) + * @param model_name name that identifies model in gazebo * @param enable 1 to enable gravity, 0 otherwise * @return returns true or false on success failure */ diff --git a/plugins/linkattacher/src/linkattacherserverimpl.cpp b/plugins/linkattacher/src/linkattacherserverimpl.cpp index d23a6985d..bef95b482 100644 --- a/plugins/linkattacher/src/linkattacherserverimpl.cpp +++ b/plugins/linkattacher/src/linkattacherserverimpl.cpp @@ -16,172 +16,179 @@ LinkAttacherServerImpl::LinkAttacherServerImpl() LinkAttacherServerImpl::~LinkAttacherServerImpl() {} -bool LinkAttacherServerImpl::attachUnscoped(const string& model_name, const std::string& model_link_name, const std::string& robot_name, const string& robot_link_name) +bool LinkAttacherServerImpl::attachUnscoped(const string& parent_model_name, const string& parent_model_link_name, const string& child_model_name, const string& child_model_link_name) { - #if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::ModelPtr object_model = _world->ModelByName(model_name); + gazebo::physics::ModelPtr parent_model = _world->ModelByName(parent_model_name); #else - gazebo::physics::ModelPtr object_model = _world->GetModel(model_name); + gazebo::physics::ModelPtr parent_model = _world->GetModel(parent_model_name); #endif - if(!object_model) + if(!parent_model) { - yError() << LogPrefix << "Attach: " << model_name << " does not exist in gazebo"; + yError() << LogPrefix << "Attach: " << parent_model_name << " model does not exist in gazebo"; return false; } else { - yInfo() << LogPrefix << "Attach: " << object_model->GetName() << " found"; + yInfo() << LogPrefix << "Attach: " << parent_model->GetName() << " model found"; } - gazebo::physics::LinkPtr object_link = object_model->GetLink(model_link_name); - if(!object_link) + //Get the exact link with only link name instead of full_scoped_link_name + gazebo::physics::Link_V parent_model_links = parent_model->GetLinks(); + gazebo::physics::LinkPtr parent_model_link; + for(int i=0; i < parent_model_links.size(); i++) { - yError() << LogPrefix << "Attach: " << model_link_name << " is not found"; - return false; + std::string candidate_parent_model_link_name = parent_model_links[i]->GetScopedName(); + + // hasEnding compare ending of the condidate model link name to the given link name, in order to be able to use unscoped names + if(GazeboYarpPlugins::hasEnding(candidate_parent_model_link_name, parent_model_link_name)) + { + parent_model_link = parent_model_links[i]; + yInfo() << LogPrefix << "Attach: " << parent_model_link->GetName() << " link found"; + break; + } } - else + if(!parent_model_link) { - yInfo() << LogPrefix << "Attach: " << object_link->GetName() << " found"; + yError() << LogPrefix << "Attach: " << parent_model_link_name << " link is not found"; + return false; } #if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::ModelPtr robot_model = _world->ModelByName(robot_name); + gazebo::physics::ModelPtr child_model = _world->ModelByName(child_model_name); #else - gazebo::physics::ModelPtr robot_model = _world->GetModel(robot_name); + gazebo::physics::ModelPtr child_model = _world->GetModel(child_model_name); #endif - if(!robot_model) + if(!child_model) { - yError() << LogPrefix << "Attach: " << robot_name << " does not exist in gazebo"; + yError() << LogPrefix << "Attach: " << child_model_name << " model does not exist in gazebo"; return false; } else { - yInfo() << LogPrefix << "Attach: " << robot_model->GetName() << " found"; + yInfo() << LogPrefix << "Attach: " << child_model->GetName() << " model found"; } //Get the exact link with only link name instead of full_scoped_link_name - gazebo::physics::Link_V robot_model_links = robot_model->GetLinks(); - for(int i=0; i < robot_model_links.size(); i++) + gazebo::physics::Link_V child_model_links = child_model->GetLinks(); + gazebo::physics::LinkPtr child_model_link; + for(int i=0; i < child_model_links.size(); i++) { - //E.g iCub::l_hand::l_hand_base_link - - std::string candidate_robot_link_name = robot_model_links[i]->GetScopedName(); - - std::size_t lastcolon = candidate_robot_link_name.rfind(":"); - std::string unscoped_robot_link_name = candidate_robot_link_name.substr(lastcolon+1,std::string::npos); - - if(unscoped_robot_link_name == robot_link_name) + std::string candidate_child_model_link_name = child_model_links[i]->GetScopedName(); + // hasEnding compare ending of the condidate model link name to the given link name, in order to be able to use unscoped names + if(GazeboYarpPlugins::hasEnding(candidate_child_model_link_name, child_model_link_name)) { - yInfo() << LogPrefix << "Attach: Full scoped Candidate robot link name: " << candidate_robot_link_name << " found"; - gazebo::physics::LinkPtr robot_link = robot_model_links[i]; - if(!robot_link) - { - yError() << LogPrefix << "Attach: " << robot_link_name << " is not found"; - return false; - } - else - { - yInfo() << LogPrefix << "Attach: " << robot_link->GetName() << " found"; - } - - //This is joint creation - gazebo::physics::JointPtr joint; + child_model_link = child_model_links[i]; + yInfo() << LogPrefix << "Attach: " << child_model_link->GetName() << " link found"; + break; + } + } + if(!child_model_link) + { + yError() << LogPrefix << "Attach: " << child_model_link_name << " link is not found"; + return false; + } - #if GAZEBO_MAJOR_VERSION >= 8 - joint = _world->Physics()->CreateJoint("fixed",object_model); - #else - joint = _world->GetPhysicsEngine()->CreateJoint("fixed",object_model); - #endif + //This is joint creation + gazebo::physics::JointPtr fixed_joint; - if(!joint) - { - yError() << LogPrefix << "Attach: Unable to create joint"; - return false; - } + #if GAZEBO_MAJOR_VERSION >= 8 + fixed_joint = _world->Physics()->CreateJoint("fixed",parent_model); + #else + fixed_joint = _world->GetPhysicsEngine()->CreateJoint("fixed",parent_model); + #endif - std::string joint_name = model_link_name + "_magnet_joint"; - joint->SetName(joint_name); - yInfo() << LogPrefix << "Attach: Magnet joint: " << joint->GetName() << " created"; + if(!fixed_joint) + { + yError() << LogPrefix << "Attach: Unable to create joint"; + return false; + } - joint->SetModel(object_model); + std::string joint_name = parent_model_link_name + "_fixed_joint"; + fixed_joint->SetName(joint_name); + yInfo() << LogPrefix << "Attach: fixed joint: " << fixed_joint->GetName() << " created"; - #if GAZEBO_MAJOR_VERSION >= 8 - joint->Load(object_link,robot_link,ignition::math::Pose3d()); - #else - joint->Load(object_link,robot_link,gazebo::math::Pose()); - #endif + fixed_joint->SetModel(parent_model); - //Attach(prent_link,child_link) - joint->Attach(object_link,robot_link); + #if GAZEBO_MAJOR_VERSION >= 8 + fixed_joint->Load(parent_model_link,child_model_link,ignition::math::Pose3d()); + #else + fixed_joint->Load(parent_model_link,child_model_link,gazebo::math::Pose()); + #endif - break; - } - } + //Attach(parent_link,child_link) + fixed_joint->Attach(parent_model_link,child_model_link); return true; } -bool LinkAttacherServerImpl::detachUnscoped(const string& model_name, const std::string& model_link_name) +bool LinkAttacherServerImpl::detachUnscoped(const string& model_name, const string& model_link_name) { #if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::ModelPtr object_model = _world->ModelByName(model_name); + gazebo::physics::ModelPtr model = _world->ModelByName(model_name); #else - gazebo::physics::ModelPtr object_model = _world->GetModel(model_name); + gazebo::physics::ModelPtr model = _world->GetModel(model_name); #endif - if(!object_model) + if(!model) { - yError() << LogPrefix << "Detach: " << model_name << " does not exist in gazebo"; + yError() << LogPrefix << "Detach: " << model_name << " model does not exist in gazebo"; return false; } else { - yInfo() << LogPrefix << "Detach: " << object_model->GetName() << " found"; + yInfo() << LogPrefix << "Detach: " << model->GetName() << " model found"; } - gazebo::physics::LinkPtr object_link = object_model->GetLink(model_link_name); - if(!object_link) + //Get the exact link with only link name instead of full_scoped_link_name + gazebo::physics::Link_V model_links = model->GetLinks(); + gazebo::physics::LinkPtr model_link; + for(int i=0; i < model_links.size(); i++) { - yError() << LogPrefix << "Detach: " << model_link_name << " is not found"; - return false; + std::string candidate_model_link_name = model_links[i]->GetScopedName(); + // hasEnding compare ending of the condidate model link name to the given link name, in order to be able to use unscoped names + if(GazeboYarpPlugins::hasEnding(candidate_model_link_name, model_link_name)) + { + model_link = model_links[i]; + yInfo() << LogPrefix << "Detach: " << model_link->GetName() << " link found"; + break; + } } - else + if(!model_link) { - yInfo() << LogPrefix << "Detach: " << object_link->GetName() << " found"; + yError() << LogPrefix << "Detach: " << model_link_name << " link is not found"; + return false; } - std::string joint_name = model_name + "::" + model_link_name + "_magnet_joint"; + std::string joint_name = model_name + "::" + model_link_name + "_fixed_joint"; //Get all the joints at the object link - gazebo::physics::Joint_V joints_v = object_link->GetChildJoints(); - + gazebo::physics::Joint_V joints_v = model_link->GetChildJoints(); + gazebo::physics::JointPtr fixed_joint; for(int i=0; i < joints_v.size(); i++) { std::string candidate_joint_name = joints_v[i]->GetScopedName(); if(candidate_joint_name == joint_name) { - gazebo::physics::JointPtr joint = joints_v[i]; - - if(!joint) + fixed_joint = joints_v[i]; + if(fixed_joint) { - yError() << LogPrefix << "Detach: Magnet Joint not found"; - return false; - } - else - { - joint->Detach(); - yInfo() << LogPrefix << "Detach: Found joint: " << joint->GetName() << " detached"; + fixed_joint->Detach(); + yInfo() << LogPrefix << "Detach: Found joint: " << fixed_joint->GetName() << " detached"; } } } + if(!fixed_joint) + { + yError() << LogPrefix << "Detach: fixed Joint not found"; + return false; + } return true; - } -bool LinkAttacherServerImpl::enableGravity(const std::string& model_name, const bool enable) +bool LinkAttacherServerImpl::enableGravity(const string& model_name, const bool enable) { #if GAZEBO_MAJOR_VERSION >= 8 gazebo::physics::ModelPtr model = _world->ModelByName(model_name); From bea7588f829f49809e025349682ee348c1dde7b1 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Fri, 9 Nov 2018 15:22:29 +0100 Subject: [PATCH 03/25] Add use of loadConfigModelPlugin() --- AUTHORS | 3 +- plugins/basestate/src/BaseState.cc | 48 +++++++--------- .../src/ContactLoadCellArray.cc | 56 +++++++------------ plugins/controlboard/src/ControlBoard.cc | 51 ++++++----------- .../fakecontrolboard/src/FakeControlBoard.cc | 48 +++++++--------- .../inertialmtbPart/src/inertialMTBPart.cc | 4 +- plugins/jointsensors/src/JointSensors.cc | 8 ++- plugins/linkattacher/src/linkattacher.cc | 55 ++++++++---------- plugins/maissensor/src/MaisSensor.cc | 53 ++++++------------ plugins/videotexture/src/VideoTexture.cc | 18 +----- 10 files changed, 130 insertions(+), 214 deletions(-) diff --git a/AUTHORS b/AUTHORS index 5b2d85255..684f9c50f 100644 --- a/AUTHORS +++ b/AUTHORS @@ -19,4 +19,5 @@ Mirko Ferrati Nicola Piga Prashanth Ramadoss Silvio Traversaro -Yeshasvi Tirupachuri +Yeshasvi Tirupachuri +Lorenzo Rapetti diff --git a/plugins/basestate/src/BaseState.cc b/plugins/basestate/src/BaseState.cc index d8135d1a7..187a96d7b 100644 --- a/plugins/basestate/src/BaseState.cc +++ b/plugins/basestate/src/BaseState.cc @@ -65,33 +65,27 @@ namespace gazebo ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::GazeboYarpBaseStateDriver> ("gazebo_basestate", "analogServer", "GazeboYarpBaseState")); - yarp::os::Bottle networkDeviceProp; - yarp::os::Bottle deviceDriverProp; - - if (_sdf->HasElement("yarpConfigurationFile")) - { - std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); - std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - - GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_parent, _sdf, m_config); - - bool wipe = false; - if (ini_file_path != "" && m_config.fromConfigFile(ini_file_path.c_str(), wipe)) - { - networkDeviceProp = m_config.findGroup("WRAPPER"); - if (networkDeviceProp.isNull()) - { - yError() << "GazeboYarpBaseState plugin failed: [WRAPPER] group not found in config file"; - return; - } - - deviceDriverProp = m_config.findGroup("DRIVER"); - if (deviceDriverProp.isNull()) - { - yError() << "GazeboYarpBaseState plugin failed: [DRIVER] group not found in config file"; - return; - } - } + // Getting .ini configuration file parameters from sdf + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, m_config); + + if (!configuration_loaded) + { + yError() << "GazeboYarpBaseState : File .ini not found, load failed." ; + return; + } + + yarp::os::Bottle networkDeviceProp = m_config.findGroup("WRAPPER"); + if (networkDeviceProp.isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: [WRAPPER] group not found in config file"; + return; + } + + yarp::os::Bottle deviceDriverProp = m_config.findGroup("DRIVER"); + if (deviceDriverProp.isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: [DRIVER] group not found in config file"; + return; } if (networkDeviceProp.find("device").isNull()) diff --git a/plugins/contactloadcellarray/src/ContactLoadCellArray.cc b/plugins/contactloadcellarray/src/ContactLoadCellArray.cc index b8109d33a..a58b25e17 100644 --- a/plugins/contactloadcellarray/src/ContactLoadCellArray.cc +++ b/plugins/contactloadcellarray/src/ContactLoadCellArray.cc @@ -74,46 +74,30 @@ namespace gazebo // Add the gazebo device driver to the factory ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::GazeboYarpContactLoadCellArrayDriver> ("gazebo_contactloadcellarray", "analogServer", "GazeboYarpContactLoadCellArray")); - - // Getting .ini config file from _sdf - yarp::os::Bottle wrapper_properties; - yarp::os::Bottle driver_properties; - - bool configuration_loaded = false; - if (_sdf->HasElement("yarpConfigurationFile")) - { - std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); - std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - - GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_parent, _sdf, m_parameters); - - bool wipe = false; - if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str(), wipe)) - { - wrapper_properties = m_parameters.findGroup("WRAPPER"); - if (wrapper_properties.isNull()) - { - yError() << "GazeboYarpContactLoadCellArray Plugin failed: [WRAPPER] group not found in config file"; - return; - } - - driver_properties = m_parameters.findGroup("DRIVER"); - if (driver_properties.isNull()) - { - yError() << "GazeboYarpContactLoadCellArray Plugin failed: [DRIVER] group not found in config file"; - return; - } - - configuration_loaded = true; - } - } - + + // Getting .ini configuration file parameters from sdf + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, m_parameters); + if (!configuration_loaded) { - yError() << "GazeboYarpContactLoadCellArray Plugin Failed: File .ini not found, load failed"; + yError() << "GazeboYarpContactLoadCellArray : File .ini not found, load failed." ; return; } - + + yarp::os::Bottle wrapper_properties = m_parameters.findGroup("WRAPPER"); + if (wrapper_properties.isNull()) + { + yError() << "GazeboYarpContactLoadCellArray Plugin failed: [WRAPPER] group not found in config file"; + return; + } + + yarp::os::Bottle driver_properties = m_parameters.findGroup("DRIVER"); + if (driver_properties.isNull()) + { + yError() << "GazeboYarpContactLoadCellArray Plugin failed: [DRIVER] group not found in config file"; + return; + } + // Check on Required Parameter for Analog Sensor Wrapper if (wrapper_properties.find("device").isNull()) { diff --git a/plugins/controlboard/src/ControlBoard.cc b/plugins/controlboard/src/ControlBoard.cc index 16fb6b5ee..6c3b84901 100644 --- a/plugins/controlboard/src/ControlBoard.cc +++ b/plugins/controlboard/src/ControlBoard.cc @@ -68,45 +68,29 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) // Add the gazebo_controlboard device driver to the factory. yarp::dev::Drivers::factory().add(new yarp::dev::DriverCreatorOf("gazebo_controlboard", "controlboardwrapper2", "GazeboYarpControlBoardDriver")); - //Getting .ini configuration file from sdf - bool configuration_loaded = false; - - yarp::os::Bottle wrapper_group; - yarp::os::Bottle driver_group; - if (_sdf->HasElement("yarpConfigurationFile")) { - std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); - std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - - GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_parent,_sdf,m_parameters); - - bool wipe = false; - if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str(),wipe)) - { - m_parameters.put("gazebo_ini_file_path",ini_file_path.c_str()); - - wrapper_group = m_parameters.findGroup("WRAPPER"); - if(wrapper_group.isNull()) { - yError("GazeboYarpControlBoard : [WRAPPER] group not found in config file\n"); - return; - } - - if(m_parameters.check("ROS")) - { - std::string ROS; - ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); - wrapper_group.append(yarp::os::Bottle(ROS)); - } - - configuration_loaded = true; - } + // Getting .ini configuration file parameters from sdf + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, m_parameters); + if (!configuration_loaded) + { + yError() << "GazeboYarpControlBoard : File .ini not found, load failed." ; + return; } - if (!configuration_loaded) { - yError() << "GazeboYarpControlBoard : File .ini not found, load failed." ; + yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER"); + if(wrapper_group.isNull()) + { + yError("GazeboYarpControlBoard : [WRAPPER] group not found in config file\n"); return; } + if(m_parameters.check("ROS")) + { + std::string ROS; + ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); + wrapper_group.append(yarp::os::Bottle(ROS)); + } + m_wrapper.open(wrapper_group); if (!m_wrapper.isValid()) { @@ -128,6 +112,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) return; } + yarp::os::Bottle driver_group; for (int n = 0; n < netList->size(); n++) { yarp::dev::PolyDriverDescriptor newPoly; diff --git a/plugins/fakecontrolboard/src/FakeControlBoard.cc b/plugins/fakecontrolboard/src/FakeControlBoard.cc index 614ad1f76..7659226ea 100644 --- a/plugins/fakecontrolboard/src/FakeControlBoard.cc +++ b/plugins/fakecontrolboard/src/FakeControlBoard.cc @@ -68,38 +68,27 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpFakeControlBoard) // Add the gazebo_controlboard device driver to the factory. yarp::dev::Drivers::factory().add(new yarp::dev::DriverCreatorOf("gazebo_fakecontrolboard", "controlboardwrapper2", "GazeboYarpFakeControlBoardDriver")); - //Getting .ini configuration file from sdf - bool configuration_loaded = false; + // Getting .ini configuration file parameters from sdf + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, m_pluginParameters); - yarp::os::Bottle wrapper_group; - yarp::os::Bottle driver_group; - if (_sdf->HasElement("yarpConfigurationFile")) { - std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); - std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - - GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_parent,_sdf,m_pluginParameters); - - bool wipe = false; - if (ini_file_path != "" && m_pluginParameters.fromConfigFile(ini_file_path.c_str(),wipe)) - { - m_pluginParameters.put("gazebo_ini_file_path",ini_file_path.c_str()); - - wrapper_group = m_pluginParameters.findGroup("WRAPPER"); - if(wrapper_group.isNull()) { - yError("GazeboYarpFakeControlBoard : [WRAPPER] group not found in config file\n"); - return; - } - - if(m_pluginParameters.check("ROS")) - { - std::string ROS; - ROS = std::string ("(") + m_pluginParameters.findGroup("ROS").toString() + std::string (")"); - wrapper_group.append(yarp::os::Bottle(ROS)); - } + if (!configuration_loaded) + { + yError() << "GazeboYarpFakeControlBoard : File .ini not found, load failed." ; + return; + } - configuration_loaded = true; - } + yarp::os::Bottle wrapper_group = m_pluginParameters.findGroup("WRAPPER"); + if(wrapper_group.isNull()) + { + yError("GazeboYarpFakeControlBoard : [WRAPPER] group not found in config file\n"); + return; + } + if(m_pluginParameters.check("ROS")) + { + std::string ROS; + ROS = std::string ("(") + m_pluginParameters.findGroup("ROS").toString() + std::string (")"); + wrapper_group.append(yarp::os::Bottle(ROS)); } if (!configuration_loaded) { @@ -128,6 +117,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpFakeControlBoard) return; } + yarp::os::Bottle driver_group; for (int n = 0; n < netList->size(); n++) { yarp::dev::PolyDriverDescriptor newPoly; diff --git a/plugins/inertialmtbPart/src/inertialMTBPart.cc b/plugins/inertialmtbPart/src/inertialMTBPart.cc index aa405db5a..60c9a22c7 100644 --- a/plugins/inertialmtbPart/src/inertialMTBPart.cc +++ b/plugins/inertialmtbPart/src/inertialMTBPart.cc @@ -55,10 +55,10 @@ void GazeboYarpInertialMTBPart::Load(physics::ModelPtr _parent, sdf::ElementPtr ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::GazeboYarpInertialMTBPartDriver> ("gazebo_inertialMTB", "analogServer", "GazeboYarpInertialMTBPartDriver")); - //Getting .ini configuration file parameters from sdf + // Getting .ini configuration file parameters from sdf ::yarp::os::Property plugin_properties; - bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent,_sdf,plugin_properties); + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, plugin_properties); if (!configuration_loaded) { diff --git a/plugins/jointsensors/src/JointSensors.cc b/plugins/jointsensors/src/JointSensors.cc index 42cd71558..55e5e6d89 100644 --- a/plugins/jointsensors/src/JointSensors.cc +++ b/plugins/jointsensors/src/JointSensors.cc @@ -57,13 +57,15 @@ void GazeboYarpJointSensors::Load(physics::ModelPtr _parent, sdf::ElementPtr _sd ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::GazeboYarpJointSensorsDriver> ("gazebo_jointsensors", "analogServer", "GazeboYarpJointSensorsDriver")); - //Getting .ini configuration file from sdf + // Getting .ini configuration file from sdf ::yarp::os::Property wrapper_properties; ::yarp::os::Property driver_properties; - bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent,_sdf,driver_properties); + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, driver_properties); - if (!configuration_loaded) { + if (!configuration_loaded) + { + yError() << "GazeboYarpJointSensors : File .ini not found, load failed." ; return; }; diff --git a/plugins/linkattacher/src/linkattacher.cc b/plugins/linkattacher/src/linkattacher.cc index bf42ac362..4c64392af 100644 --- a/plugins/linkattacher/src/linkattacher.cc +++ b/plugins/linkattacher/src/linkattacher.cc @@ -44,44 +44,33 @@ void LinkAttacher::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) m_la_server.attachWorldPointer(_model->GetWorld()); m_la_server.attachModelPointer(_model); - bool configuration_loaded = false; + // Getting .ini configuration file parameters from sdf + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_model, _sdf, m_parameters); - if( _sdf->HasElement("yarpConfigurationFile") ) + if (!configuration_loaded) { - std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); - std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - - if(!ini_file_path.empty() && m_parameters.fromConfigFile(ini_file_path.c_str())) - { - yInfo() << LogPrefix << "Load: Found yarpConfigurationFile: loading from " << ini_file_path; - configuration_loaded = true; - } - - if(!configuration_loaded) - { - yError() << LogPrefix << "Load: Failed to load the configuration file"; + yError() << LogPrefix << " File .ini not found, load failed." ; return; - } - - std::string portname; - if(m_parameters.check("name")) - { - portname = m_parameters.find("name").asString(); - } - else - { - yError() << LogPrefix << "Load: name parameter missing in the configuration file"; - return; - } + } - m_rpcport = std::unique_ptr(new yarp::os::RpcServer()); - if(!m_rpcport->open(portname)) - { - yError() << LogPrefix << "Load: failed to open rpcserver port"; - return; - } + std::string portname; + if(m_parameters.check("name")) + { + portname = m_parameters.find("name").asString(); + } + else + { + yError() << LogPrefix << " parameter missing in the configuration file"; + return; + } - m_la_server.yarp().attachAsServer(*m_rpcport); + m_rpcport = std::unique_ptr(new yarp::os::RpcServer()); + if(!m_rpcport->open(portname)) + { + yError() << LogPrefix << " failed to open rpcserver port"; + return; } + m_la_server.yarp().attachAsServer(*m_rpcport); + } diff --git a/plugins/maissensor/src/MaisSensor.cc b/plugins/maissensor/src/MaisSensor.cc index 68881ea39..075e10435 100644 --- a/plugins/maissensor/src/MaisSensor.cc +++ b/plugins/maissensor/src/MaisSensor.cc @@ -64,46 +64,28 @@ void GazeboYarpMaisSensor::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) // Add the gazebo_controlboard device driver to the factory. yarp::dev::Drivers::factory().add(new yarp::dev::DriverCreatorOf("gazebo_maissensor", "analogServer", "GazeboYarpMaisSensorDriver")); - //Getting .ini configuration file from sdf - bool configuration_loaded = false; - - yarp::os::Bottle wrapper_group; - yarp::os::Bottle driver_group; - if (_sdf->HasElement("yarpConfigurationFile")) { - std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); - std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - - GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_parent,_sdf,m_parameters); - - bool wipe = false; - if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str(),wipe)) - { - yInfo() << "GazeboYarpMaisSensor: Found yarpConfigurationFile: loading from " << ini_file_path; - m_parameters.put("gazebo_ini_file_path",ini_file_path.c_str()); - - wrapper_group = m_parameters.findGroup("WRAPPER"); - if(wrapper_group.isNull()) - { - yError("GazeboYarpMaisSensor : [WRAPPER] group not found in config file\n"); - return; - } - - if(m_parameters.check("ROS")) - { - std::string ROS; - ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); - wrapper_group.append(yarp::os::Bottle(ROS)); - } - - configuration_loaded = true; - } + // Getting .ini configuration file parameters from sdf + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, m_parameters); + if (!configuration_loaded) + { + yError() << "GazeboYarpMaisSensor : File .ini not found, load failed." ; + return; } - if (!configuration_loaded) { - yError() << "GazeboYarpMaisSensor: File .ini not found, quitting" ; + yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER"); + if(wrapper_group.isNull()) + { + yError("GazeboYarpMaisSensor : [WRAPPER] group not found in config file\n"); return; } + + if(m_parameters.check("ROS")) + { + std::string ROS; + ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); + wrapper_group.append(yarp::os::Bottle(ROS)); + } //Open the wrapper if( m_wrapper.open(wrapper_group) ) @@ -137,6 +119,7 @@ void GazeboYarpMaisSensor::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) m_sensorName = m_robotName + "::" + newPoly.key.c_str(); newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(m_sensorName); + yarp::os::Bottle driver_group; if( newPoly.poly != NULL) { // device already exists, use it, setting it againg to increment the usage counter. diff --git a/plugins/videotexture/src/VideoTexture.cc b/plugins/videotexture/src/VideoTexture.cc index 55bc9089e..15552d143 100644 --- a/plugins/videotexture/src/VideoTexture.cc +++ b/plugins/videotexture/src/VideoTexture.cc @@ -78,29 +78,17 @@ namespace gazebo return; } - //Getting .ini configuration file from sdf - bool configuration_loaded = false; - + // Getting .ini configuration file parameters from sdf if(!sdf->HasElement("yarpConfigurationFile") && sdf->HasElement("sdf")) { sdf = sdf->GetElement("sdf"); } - if (sdf->HasElement("yarpConfigurationFile")) - { - std::string ini_file_name = sdf->Get("yarpConfigurationFile"); - std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - - if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str())) - { - //yInfo() << "Found yarpConfigurationFile: loading from " << ini_file_path ; - configuration_loaded = true; - } - } + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, m_parameters); if (!configuration_loaded) { - //yError() << "VideoTexture::Load error could not load configuration file"; + yError() << "VideoTexture : File .ini not found, load failed." ; return; } From 32e7252e7147ebb1729fee5a4c1284e249965feb Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Mon, 12 Nov 2018 14:08:36 +0100 Subject: [PATCH 04/25] Add parameter --- .../include/GazeboYarpPlugins/ConfHelpers.hh | 8 ++- libraries/singleton/src/ConfHelpers.cc | 58 +++++++++++++------ 2 files changed, 45 insertions(+), 21 deletions(-) diff --git a/libraries/singleton/include/GazeboYarpPlugins/ConfHelpers.hh b/libraries/singleton/include/GazeboYarpPlugins/ConfHelpers.hh index 517107215..33bfb0656 100644 --- a/libraries/singleton/include/GazeboYarpPlugins/ConfHelpers.hh +++ b/libraries/singleton/include/GazeboYarpPlugins/ConfHelpers.hh @@ -45,7 +45,9 @@ std::vector splitString(const std::string &s, const std::string &de * Load the configuration for a given model plugin, * and save the configuration in the plugin_parameters output object. * This involves calling addGazeboEnviromentalVariablesModel and then loading - * the yarp configuration file specified in the sdf with the yarpConfigurationFile tag. + * the yarp configuration file specified in the sdf with the yarpConfigurationFile and + * yarpConfigurationString tags. In case a parameter is defined in both, the yarpConfigurationString + * has the priority. */ bool loadConfigModelPlugin(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf, @@ -74,7 +76,9 @@ bool addGazeboEnviromentalVariablesModel(gazebo::physics::ModelPtr _parent, * Load the configuration for a given sensor plugin, * and save the configuration in the plugin_parameters output object. * This involves calling addGazeboEnviromentalVariablesSensor and then loading - * the yarp configuration file specified in the sdf with the yarpConfigurationFile tag. + * the yarp configuration file specified in the sdf with the yarpConfigurationFile and + * yarpConfigurationString tags. In case a parameter is defined in both, the yarpConfigurationString + * has the priority. */ bool loadConfigSensorPlugin(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf, diff --git a/libraries/singleton/src/ConfHelpers.cc b/libraries/singleton/src/ConfHelpers.cc index 6704abd54..a04a67b6a 100644 --- a/libraries/singleton/src/ConfHelpers.cc +++ b/libraries/singleton/src/ConfHelpers.cc @@ -60,23 +60,33 @@ bool loadConfigModelPlugin(physics::ModelPtr _model, sdf::ElementPtr _sdf, yarp::os::Property& plugin_parameters) { - if (_sdf->HasElement("yarpConfigurationFile")) { + GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_model,_sdf,plugin_parameters); + bool wipe = false; + bool loaded_configuration = true; + + if (_sdf->HasElement("yarpConfigurationFile")) + { std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_model,_sdf,plugin_parameters); - - bool wipe = false; if (ini_file_path != "" && plugin_parameters.fromConfigFile(ini_file_path.c_str(),wipe)) { - return true; + loaded_configuration = true; } else { yError() << "GazeboYarpPlugins error: failure in loading configuration for model" << _model->GetName() << "\n" << "GazeboYarpPlugins error: yarpConfigurationFile : " << ini_file_name << "\n" << "GazeboYarpPlugins error: yarpConfigurationFile absolute path : " << ini_file_path; - return false; + loaded_configuration = false; } } - return true; + + if (_sdf->HasElement("yarpConfigurationString")) + { + std::string configuration_string = _sdf->Get("yarpConfigurationString"); + plugin_parameters.fromString(configuration_string, wipe); + yInfo() << "GazeboYarpPlugins: configuration of model " << _model->GetName() << " loaded from yarpConfigurationString : " << configuration_string << "\n"; + } + + return loaded_configuration; } bool addGazeboEnviromentalVariablesSensor(gazebo::sensors::SensorPtr _sensor, @@ -122,31 +132,41 @@ bool loadConfigSensorPlugin(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf, yarp::os::Property& plugin_parameters) { + GazeboYarpPlugins::addGazeboEnviromentalVariablesSensor(_sensor,_sdf,plugin_parameters); + bool wipe = false; + bool loaded_configuration = true; + +#if GAZEBO_MAJOR_VERSION >= 7 + std::string sensorName = _sensor->Name(); +#else + std::string sensorName = _sensor->GetName(); +#endif + if (_sdf->HasElement("yarpConfigurationFile")) { std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - - GazeboYarpPlugins::addGazeboEnviromentalVariablesSensor(_sensor,_sdf,plugin_parameters); - - bool wipe = false; + if (ini_file_path != "" && plugin_parameters.fromConfigFile(ini_file_path.c_str(),wipe)) { - return true; + loaded_configuration = true; } else { -#if GAZEBO_MAJOR_VERSION >= 7 - std::string sensorName = _sensor->Name(); -#else - std::string sensorName = _sensor->GetName(); -#endif yError() << "GazeboYarpPlugins error: failure in loading configuration for sensor " << sensorName << "\n" << "GazeboYarpPlugins error: yarpConfigurationFile : " << ini_file_name << "\n" << "GazeboYarpPlugins error: yarpConfigurationFile absolute path : " << ini_file_path ; - return false; + loaded_configuration = false; } } - return true; + + if (_sdf->HasElement("yarpConfigurationString")) + { + std::string configuration_string = _sdf->Get("yarpConfigurationString"); + plugin_parameters.fromString(configuration_string, wipe); + yInfo() << "GazeboYarpPlugins: configuration of sensor " << sensorName << " loaded from yarpConfigurationString : " << configuration_string << "\n"; + } + + return loaded_configuration; } From 50aeb9307daeb2c35d8b1def1616c5157da3a53b Mon Sep 17 00:00:00 2001 From: Lorenzo Rapetti Date: Mon, 12 Nov 2018 15:21:40 +0100 Subject: [PATCH 05/25] Update embed_plugins.md --- doc/embed_plugins.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/doc/embed_plugins.md b/doc/embed_plugins.md index b1cc240a0..754fda5a2 100644 --- a/doc/embed_plugins.md +++ b/doc/embed_plugins.md @@ -55,7 +55,10 @@ For model plugins such as the `gazebo_yarp_controlboard`, a plugin can be loaded ~~~ Most Model and Sensor plugins present in `gazebo-yarp-plugins` read their configuration from the file referenced in the `yarpConfigurationFile` tag. -The location of the file is defined by using a [Gazebo URI](https://bitbucket.org/osrf/gazebo/wiki/uri), while the file is a [.ini YARP configuration file](http://www.yarp.it/yarp_config_files.html). +The location of the file is defined by using a [Gazebo URI](https://bitbucket.org/osrf/gazebo/wiki/uri), while the file is a [.ini YARP configuration file](http://www.yarp.it/yarp_config_files.html). Alternatively, parameters can be passed directly in the `sdf` with the [`Bottle`](http://www.yarp.it/classyarp_1_1os_1_1Bottle.html) format using the `yarpConfigurationString` tag (e.g. `(name /iCub/linkattacher/rpc:i)`). When using `yarpConfigurationString`, it is important to remember that: +- only **one** `yarpConfigurationString` can be defined for each plugin. +- the tags `` and `` can coexist, but if a parameter is defined in both the value in `` will **always** be used. + For specific information consumed by each plugin, please refer to the doxygen documentation of the specific plugin class. System plugins instead need to be loaded at the beginning of the simulation, and hence they are passed as command line arguments when launching `gazebo` (or `gzserver`). From a66de0ca697090a5fbd53f6f8338c9d640faea87 Mon Sep 17 00:00:00 2001 From: Lorenzo Rapetti Date: Sat, 17 Nov 2018 14:59:57 +0100 Subject: [PATCH 06/25] Fix loadConfigModelPlugin inputs in VideoTexture #397 --- plugins/videotexture/src/VideoTexture.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/videotexture/src/VideoTexture.cc b/plugins/videotexture/src/VideoTexture.cc index 15552d143..8122664ca 100644 --- a/plugins/videotexture/src/VideoTexture.cc +++ b/plugins/videotexture/src/VideoTexture.cc @@ -84,7 +84,7 @@ namespace gazebo sdf = sdf->GetElement("sdf"); } - bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, m_parameters); + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(parent, sdf, m_parameters); if (!configuration_loaded) { From b70b1b40b7536ffafd2603c95b2a1278a144433e Mon Sep 17 00:00:00 2001 From: Lorenzo Rapetti Date: Sat, 17 Nov 2018 19:26:24 +0100 Subject: [PATCH 07/25] Revert #394 in VideoTexture plugin --- plugins/videotexture/src/VideoTexture.cc | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/plugins/videotexture/src/VideoTexture.cc b/plugins/videotexture/src/VideoTexture.cc index 8122664ca..052935c0b 100644 --- a/plugins/videotexture/src/VideoTexture.cc +++ b/plugins/videotexture/src/VideoTexture.cc @@ -78,13 +78,25 @@ namespace gazebo return; } - // Getting .ini configuration file parameters from sdf + //Getting .ini configuration file from sdf + bool configuration_loaded = false; + if(!sdf->HasElement("yarpConfigurationFile") && sdf->HasElement("sdf")) { sdf = sdf->GetElement("sdf"); } - bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(parent, sdf, m_parameters); + if (sdf->HasElement("yarpConfigurationFile")) + { + std::string ini_file_name = sdf->Get("yarpConfigurationFile"); + std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); + + if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str())) + { + //yInfo() << "Found yarpConfigurationFile: loading from " << ini_file_path ; + configuration_loaded = true; + } + } if (!configuration_loaded) { From 1023891d67242392589a26ab5cbd2bc670424394 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Mon, 5 Nov 2018 18:39:10 +0100 Subject: [PATCH 08/25] Add configurationOverride Plugin --- CMakeLists.txt | 2 +- doc/embed_plugins.md | 20 +++-- plugins/CMakeLists.txt | 1 + plugins/configurationoverride/CMakeLists.txt | 23 +++++ plugins/configurationoverride/README.md | 27 ++++++ .../include/gazebo/ConfigurationOverride.hh | 31 +++++++ .../src/ConfigurationOverride.cc | 87 +++++++++++++++++++ 7 files changed, 181 insertions(+), 10 deletions(-) create mode 100644 plugins/configurationoverride/CMakeLists.txt create mode 100644 plugins/configurationoverride/README.md create mode 100644 plugins/configurationoverride/include/gazebo/ConfigurationOverride.hh create mode 100644 plugins/configurationoverride/src/ConfigurationOverride.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index f7e86f339..6b90626fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ PROJECT(GazeboYARPPlugins) # Project version set(${PROJECT_NAME}_MAJOR_VERSION 3) set(${PROJECT_NAME}_MINOR_VERSION 1) -set(${PROJECT_NAME}_PATCH_VERSION 0) +set(${PROJECT_NAME}_PATCH_VERSION 101) set(${PROJECT_NAME}_VERSION ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION}) diff --git a/doc/embed_plugins.md b/doc/embed_plugins.md index 754fda5a2..68eab4cf3 100644 --- a/doc/embed_plugins.md +++ b/doc/embed_plugins.md @@ -33,16 +33,18 @@ but are nevertheless useful for simulating complex scenarios. | Contact Load Cell Array| `gazebo_contactloadcellarray` | Model | gazebo::GazeboYarpContactLoadCellArray | yarp::dev::GazeboYarpContactLoadCellArrayDriver | ## Plugins exposing simulation-specific functionalities -| Functionality | Plugin Name | Plugin Type | Gazebo Plugin class | -| :----------------: |:-------------:| :-----:|:---------------------------------:| -| Clock synchronization | `gazebo_yarp_clock` | System | gazebo::GazeboYarpClock | -| Apply external force/torque via YARP facilities | `gazebo_yarp_externalwrench` | Model | gazebo::ApplyExternalWrench | -| Display the Center of Mass of a model | `gazebo_yarp_showmodelcom` | Model | gazebo::ShowModelCoM | -| Project an image stream on a simulated surface | `gazebo_yarp_videotexture` | Visual | gazebo::VideoTexture | -| Expose a YARP RPC interface to create/manipulate objects programatically. | `gazebo_yarp_worldinterface` | Model | gazebo::WorldInterface | -| Publish the absolute pose of the root link of a model. | `gazebo_yarp_modelposepublisher` | Model | gazebo::GazeboYarpModelPosePublisher | -| Expose a YARP RPC interface to attach/detach links of the models spawned to the links of the robot using a fixed joint. | `gazebo_yarp_rpc_linkattacher` | Model | gazebo::GazeboYarpLinkAttacher | +| Functionality | Plugin Name | Plugin Type | Gazebo Plugin class | YARP Device class (if any) | +| :----------------: |:-------------:| :-----:|:---------------------------------:|:-------------------:| +| Clock synchronization | `gazebo_yarp_clock` | System | gazebo::GazeboYarpClock | | +| Apply external force/torque via YARP facilities | `gazebo_yarp_externalwrench` | Model | gazebo::ApplyExternalWrench | | +| Display the Center of Mass of a model | `gazebo_yarp_showmodelcom` | Model | gazebo::ShowModelCoM | | +| Project an image stream on a simulated surface | `gazebo_yarp_videotexture` | Visual | gazebo::VideoTexture | | +| Expose a YARP RPC interface to create/manipulate objects programatically. | `gazebo_yarp_worldinterface` | Model | gazebo::WorldInterface | | +| Publish the absolute pose of the root link of a model. | `gazebo_yarp_modelposepublisher` | Model | gazebo::GazeboYarpModelPosePublisher | | +| Expose a YARP RPC interface to attach/detach links of the models spawned to the links of the robot using a fixed joint. | `gazebo_yarp_rpc_linkattacher` | Model | gazebo::GazeboYarpLinkAttacher | | | Exposes the absolute pose, velocity and acceleration of a desired link through YARP analog server device| `gazebo_basestate` | Model | gazebo::GazeboYarpBaseState | yarp::dev::GazeboYarpBaseStateDriver | +| Changes the value of the property of a specific plugin attached to the model (if not present, the configuration is created) | `gazebo_yarp_configurationoverride` | Model | gazebo::GazeboYarpConfigurationOverride | | + ## Using the plugins in Gazebo Models In Gazebo, the simulated models are described using the [SDF (simulation description format)]((http://gazebosim.org/sdf.html), an XML-based file format. diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt index 08c4b5eaa..838882c25 100644 --- a/plugins/CMakeLists.txt +++ b/plugins/CMakeLists.txt @@ -36,3 +36,4 @@ add_subdirectory(depthCamera) add_subdirectory(contactloadcellarray) add_subdirectory(modelposepublisher) add_subdirectory(basestate) +add_subdirectory(configurationoverride) \ No newline at end of file diff --git a/plugins/configurationoverride/CMakeLists.txt b/plugins/configurationoverride/CMakeLists.txt new file mode 100644 index 000000000..2fca925ea --- /dev/null +++ b/plugins/configurationoverride/CMakeLists.txt @@ -0,0 +1,23 @@ +# Copyright (C) 2018 Istituto Italiano di Tecnologia iCub Facility +# Authors: Lorenzo Rapetti +# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + +cmake_minimum_required(VERSION 3.5) + +PROJECT(Plugin_ConfigurationOverride) + +include(AddGazeboYarpPluginTarget) + +set(configurationoverride_sources src/ConfigurationOverride.cc) +set(configurationoverride_headers include/gazebo/ConfigurationOverride.hh) + +set(LIB_COMMON_NAME gazebo_yarp_lib_common) + +add_gazebo_yarp_plugin_target(LIBRARY_NAME configurationoverride + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR} + INCLUDE_DIRS include/gazebo include/GazeboYarpPlugins + SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} + LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + HEADERS ${configurationoverride_headers} + SOURCES ${configurationoverride_sources} + ) \ No newline at end of file diff --git a/plugins/configurationoverride/README.md b/plugins/configurationoverride/README.md new file mode 100644 index 000000000..1792a27e9 --- /dev/null +++ b/plugins/configurationoverride/README.md @@ -0,0 +1,27 @@ +### ConfigurationOverride Plugin +The ConfigurationOverride is a Gazebo _model plugin_. It can be attached to a model in order to override the [`sdf`](http://sdformat.org/spec) configuration of any other plugin that is attached to the same model. +Each ConfigurationOverride plugin points to a plugin and can override the values of multiple configuration parameters. The parameters that are not pointed in the ConfigurationOverride will remain untouched. +If the desired parameter is not found in the plugin, it is created accordingly to `configurationOverride`. + +N.B. +If a parameter is defined multiple times inside a plugin, only the first occurence will be overritten accordingly to the element retreived with [`GetElement()`](http://osrf-distributions.s3.amazonaws.com/sdformat/api/3.0/classsdf_1_1Element.html#a29fa74bb6bf29701e82b6353e732d31d). +Currently, this doesn't represent a problem since also the plugins are reading the configurations with `GetElement()`. + +### Usage +Add the ConfigurationOverride plugin to the model sdf before the overriden plugin declaration (i.e. before the ``) and define the `plugin_name` as the `name` of the plugin that will be modified. +The properties of the ConfigurationOverride plugin will be then used in order to override the properties of the desired plugin. + +e.g. If you are using the model from [`icub-gazebo`](https://github.com/robotology/icub-gazebo) and you wanto to change the `` of the `ControlBoard` (`controlboard_right_arm`), the model can be defined as follow: +```xml + + + + + -2.0 0.52 0.0 0.785 0 0 0.698 + + + model://icub + + +``` +then instead of loading the model `icub` with the standard right arm ``, the configuration defined in the ConfigurationOverride will be used. diff --git a/plugins/configurationoverride/include/gazebo/ConfigurationOverride.hh b/plugins/configurationoverride/include/gazebo/ConfigurationOverride.hh new file mode 100644 index 000000000..50b1fde67 --- /dev/null +++ b/plugins/configurationoverride/include/gazebo/ConfigurationOverride.hh @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia + * Authors: Lorenzo Rapetti. + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +#ifndef GAZEBOYARP_CONFIGURATIONOVERRIDE_HH +#define GAZEBOYARP_CONFIGURATIONOVERRIDE_HH + +#include +#include + +namespace gazebo +{ +class ConfigurationOverride : public ModelPlugin +{ +public: + ConfigurationOverride(); + virtual ~ConfigurationOverride(); + + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + +private: +}; + +//Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(ConfigurationOverride) + +} + +#endif diff --git a/plugins/configurationoverride/src/ConfigurationOverride.cc b/plugins/configurationoverride/src/ConfigurationOverride.cc new file mode 100644 index 000000000..3cd30c843 --- /dev/null +++ b/plugins/configurationoverride/src/ConfigurationOverride.cc @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia + * Authors: Lorenzo Rapetti. + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +#include + +#include + +#include +#include + +using namespace std; +using namespace gazebo; + +ConfigurationOverride::ConfigurationOverride() +{ +} + +ConfigurationOverride::~ConfigurationOverride() +{ +} + +void ConfigurationOverride::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + if (!_model) + { + gzerr << "ConfigurationOverride plugin requires a parent.\n"; + return; + } + if(! _sdf->HasElement("yarpPluginConfigurationOverride") || ! _sdf->GetElement("yarpPluginConfigurationOverride")->HasAttribute("plugin_name")) + { + yError("ConfigurationOverride : overriden_plugin_name not found in config file\n"); + return; + } + + sdf::ElementPtr overriden_element, new_element; + // name of the plugin that has to be overritten + std::string overriden_plugin_name = _sdf->GetElement("yarpPluginConfigurationOverride")->GetAttribute("plugin_name")->GetAsString(); + // sdf of the model to which the plugin is attached. the plugin to be overriden will be searched among the plugins of this model + sdf::ElementPtr model_sdf = _sdf->GetParent(); + // search for the plugin to be overriden + sdf::ElementPtr overriden_plugin = model_sdf->GetFirstElement(); + + while(overriden_plugin) + { + if(overriden_plugin->HasAttribute("name") && overriden_plugin->GetAttribute("name")->GetAsString() == overriden_plugin_name) + { + // overriden_plugin found + + // iterate the elements of the ConfigurationOverride plugin + new_element = _sdf->GetFirstElement(); + while(new_element) + { + if(new_element->GetName() != "yarpPluginConfigurationOverride") + { + // if the element to be overritten is not found in the overriden_plugin, create the element + if(! overriden_plugin->HasElement(new_element->GetName())) + { + // Create element + sdf::ElementPtr created_element(new sdf::Element); + overriden_plugin->AddElementDescription(new_element); + overriden_element = overriden_plugin->GetElement(new_element->GetName()); + } + else + { + // Override element + // write the value taken from the ConfigurationOverride element (new_element) to the overriden_plugin element (overriden_element) + overriden_element = overriden_plugin->GetElement(new_element->GetName()); + overriden_element->Set(new_element->Get()); + } + + } + new_element = new_element->GetNextElement(); + } + break; + } + overriden_plugin = overriden_plugin->GetNextElement(); + } + if(!overriden_plugin) + { + yError() <<"ConfigurationOverride : the plugin " << overriden_plugin_name << " is not found in the model " << model_sdf->GetAttribute("name")->GetAsString(); + return; + } + +} \ No newline at end of file From ac4ed33a5a493582c71e9fc6938a0ca83766a183 Mon Sep 17 00:00:00 2001 From: Prashanth Ramadoss Date: Wed, 21 Nov 2018 14:37:29 +0100 Subject: [PATCH 09/25] [virtAnalogWrapper] cleanup --- plugins/controlboard/src/ControlBoard.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/plugins/controlboard/src/ControlBoard.cc b/plugins/controlboard/src/ControlBoard.cc index 42fd83e9d..7b7b57339 100644 --- a/plugins/controlboard/src/ControlBoard.cc +++ b/plugins/controlboard/src/ControlBoard.cc @@ -81,27 +81,27 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) //Getting .ini configuration file from sdf bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, m_parameters); - + if (!configuration_loaded) { yError() << "GazeboYarpControlBoard : File .ini not found, load failed." ; return; } - + yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER"); if(wrapper_group.isNull()) { yError("GazeboYarpControlBoard : [WRAPPER] group not found in config file\n"); return; } - + if(m_parameters.check("ROS")) { std::string ROS; ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); wrapper_group.append(yarp::os::Bottle(ROS)); } - + m_wrapper.open(wrapper_group); if (!m_wrapper.isValid()) { @@ -122,7 +122,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) m_wrapper.close(); return; } - + yarp::os::Bottle driver_group; yarp::os::Bottle virt_group; From d18ff8d324f663b6ed5e8695dd61e44a11058a60 Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Wed, 21 Nov 2018 18:05:39 +0100 Subject: [PATCH 10/25] worldinterface plugin: now the makeBox function doesn't append number at the end of given name of box --- plugins/worldinterface/src/worldproxy.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/worldinterface/src/worldproxy.cpp b/plugins/worldinterface/src/worldproxy.cpp index f27b9394e..e9a83a9ac 100644 --- a/plugins/worldinterface/src/worldproxy.cpp +++ b/plugins/worldinterface/src/worldproxy.cpp @@ -257,7 +257,7 @@ string WorldProxy::makeBox(const double width, const double height, const double ostringstream objlabel; if (object_name!= "") { - objlabel << object_name << nobjects; + objlabel << object_name; } else { From b95969d159482a2f8cb47dd65e496a6956ef072f Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Wed, 21 Nov 2018 18:40:11 +0100 Subject: [PATCH 11/25] worldinterface plugin: now the makeCylinder and makeFrame functions don't append number at the end of given name of cylinder/frame --- plugins/worldinterface/src/worldproxy.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/worldinterface/src/worldproxy.cpp b/plugins/worldinterface/src/worldproxy.cpp index e9a83a9ac..4fa4c3fc0 100644 --- a/plugins/worldinterface/src/worldproxy.cpp +++ b/plugins/worldinterface/src/worldproxy.cpp @@ -371,7 +371,7 @@ string WorldProxy::makeCylinder(const double radius, const double length, const ostringstream objlabel; if (object_name!= "") { - objlabel << object_name << nobjects; + objlabel << object_name; } else { @@ -589,7 +589,7 @@ std::string WorldProxy::makeFrame(const double size, const GazeboYarpPlugins::Po ostringstream objlabel; if (object_name!= "") { - objlabel << object_name << nobjects; + objlabel << object_name; } else { From 5ccd19d74eb5442df0a99dc3427a1a773d36d685 Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Thu, 3 Jan 2019 12:57:13 +0100 Subject: [PATCH 12/25] WorldInterface: in make* functions, add check to verify if an object exixts already in gazebo --- .../src/worldinterfaceserverimpl.h | 8 +-- plugins/worldinterface/src/worldproxy.cpp | 59 ++++++++++++++++++- 2 files changed, 61 insertions(+), 6 deletions(-) diff --git a/plugins/worldinterface/src/worldinterfaceserverimpl.h b/plugins/worldinterface/src/worldinterfaceserverimpl.h index 13e0a967d..d6784f730 100644 --- a/plugins/worldinterface/src/worldinterfaceserverimpl.h +++ b/plugins/worldinterface/src/worldinterfaceserverimpl.h @@ -26,7 +26,7 @@ class WorldInterfaceServerImpl: public GazeboYarpPlugins::WorldInterfaceServer * @param radius radius of the sphere [m] * @param pose pose of the sphere [m] * @param color color of the sphere - * @return returns a string that contains the name of the object in the world + * @return returns a string that contains the name of the object in the world or empty string on error */ virtual std::string makeSphere(const double radius, const GazeboYarpPlugins::Pose& pose, const GazeboYarpPlugins::Color& color, const std::string& frame_name = "", const std::string& object_name = "",const bool gravity_enable = 0, const bool collision_enable = 1); /** @@ -36,7 +36,7 @@ class WorldInterfaceServerImpl: public GazeboYarpPlugins::WorldInterfaceServer * @param thickness box thickness [m] * @param pose pose of the box [m] * @param color color of the box - * @return returns a string that contains the name of the object in the world + * @return returns a string that contains the name of the object in the world or empty string on error */ virtual std::string makeBox(const double width, const double height, const double thickness, const GazeboYarpPlugins::Pose& pose, const GazeboYarpPlugins::Color& color, const std::string& frame_name = "", const std::string& object_name = "",const bool gravity_enable = 0, const bool collision_enable = 1); /** @@ -45,7 +45,7 @@ class WorldInterfaceServerImpl: public GazeboYarpPlugins::WorldInterfaceServer * @param length lenght of the cylinder [m] * @param pose pose of the cylinder [m] * @param color color of the cylinder - * @return returns a string that contains the name of the object in the world + * @return returns a string that contains the name of the object in the world or empty string on error */ virtual std::string makeCylinder(const double radius, const double length, const GazeboYarpPlugins::Pose& pose, const GazeboYarpPlugins::Color& color, const std::string& frame_name = "", const std::string& object_name = "",const bool gravity_enable = 0, const bool collision_enable = 1); /** @@ -87,7 +87,7 @@ class WorldInterfaceServerImpl: public GazeboYarpPlugins::WorldInterfaceServer * @param size size of the frame [m] * @param pose pose of the frame [m] * @param color color of the frame - * @return returns a string that contains the name of the object in the world + * @return returns a string that contains the name of the object in the world or empty string on error */ virtual std::string makeFrame(const double size, const GazeboYarpPlugins::Pose& pose, const GazeboYarpPlugins::Color& color, const std::string& frame_name = "", const std::string& object_name = "", const bool gravity_enable = 0, const bool collision_enable = 1); diff --git a/plugins/worldinterface/src/worldproxy.cpp b/plugins/worldinterface/src/worldproxy.cpp index 4fa4c3fc0..b02fcd1ea 100644 --- a/plugins/worldinterface/src/worldproxy.cpp +++ b/plugins/worldinterface/src/worldproxy.cpp @@ -62,6 +62,20 @@ sdf::ElementPtr getSDFRoot(sdf::SDF &sdfObj) std::string WorldProxy::makeSphere(const double radius, const GazeboYarpPlugins::Pose& pose, const GazeboYarpPlugins::Color& color, const std::string& frame_name, const std::string& object_name,const bool gravity_enable, const bool collision_enable) { + if (object_name!= "") + { + #if GAZEBO_MAJOR_VERSION >= 8 + physics::ModelPtr model=world->ModelByName(object_name); + #else + physics::ModelPtr model=world->GetModel(object_name); + #endif + if (model) + { + yError()<<"An object called " << object_name << "exists already in gazebo\n"; + return ""; + } + } + sdf::SDF sphereSDF; string sphereSDF_string=string( @@ -178,7 +192,20 @@ std::string WorldProxy::makeSphere(const double radius, const GazeboYarpPlugins: string WorldProxy::makeBox(const double width, const double height, const double thickness, const GazeboYarpPlugins::Pose& pose, const GazeboYarpPlugins::Color& color, const std::string& frame_name, const std::string& object_name,const bool gravity_enable, const bool collision_enable) { - sdf::SDF boxSDF; + if (object_name!= "") + { + #if GAZEBO_MAJOR_VERSION >= 8 + physics::ModelPtr model=world->ModelByName(object_name); + #else + physics::ModelPtr model=world->GetModel(object_name); + #endif + if (model) + { + yError()<<"An object called " << object_name << "exists already in gazebo\n"; + return ""; + } + } + sdf::SDF boxSDF; string boxSDF_String=string( "\ @@ -293,6 +320,20 @@ string WorldProxy::makeBox(const double width, const double height, const double string WorldProxy::makeCylinder(const double radius, const double length, const GazeboYarpPlugins::Pose& pose, const GazeboYarpPlugins::Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) { + if (object_name!= "") + { + #if GAZEBO_MAJOR_VERSION >= 8 + physics::ModelPtr model=world->ModelByName(object_name); + #else + physics::ModelPtr model=world->GetModel(object_name); + #endif + if (model) + { + yError()<<"An object called " << object_name << "exists already in gazebo\n"; + return ""; + } + } + sdf::SDF cylSDF; string cylSDF_String=string( @@ -476,7 +517,21 @@ bool WorldProxy::enableGravity(const std::string& id, const bool enable) std::string WorldProxy::makeFrame(const double size, const GazeboYarpPlugins::Pose& pose, const GazeboYarpPlugins::Color& color, const std::string& frame_name, const std::string& object_name,const bool gravity_enable, const bool collision_enable) { - sdf::SDF frameSDF; + if (object_name!= "") + { + #if GAZEBO_MAJOR_VERSION >= 8 + physics::ModelPtr model=world->ModelByName(object_name); + #else + physics::ModelPtr model=world->GetModel(object_name); + #endif + if (model) + { + yError()<<"An object called " << object_name << "exists already in gazebo\n"; + return ""; + } + } + + sdf::SDF frameSDF; string frameSDF_string=string( "\ From 73be7554c36bd71820df995ac357ac5994cb3266 Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Fri, 8 Feb 2019 10:38:48 +0100 Subject: [PATCH 13/25] Fix bug in deleteObject of WorldInterface plugin Due to this bug, the function deleted alway the first object in its list and not object with the name specified in the parameter of function --- plugins/worldinterface/src/worldproxy.cpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/plugins/worldinterface/src/worldproxy.cpp b/plugins/worldinterface/src/worldproxy.cpp index b02fcd1ea..ae8d96672 100644 --- a/plugins/worldinterface/src/worldproxy.cpp +++ b/plugins/worldinterface/src/worldproxy.cpp @@ -1017,27 +1017,22 @@ GazeboYarpPlugins::Pose WorldProxy::getPose(const std::string& id, const std::st bool WorldProxy::deleteObject(const std::string& id) { - ObjectsListIt it=objects.begin(); - while(it!=objects.end()) - { - string obj=it->first; #if GAZEBO_MAJOR_VERSION >= 8 - physics::ModelPtr model=world->ModelByName(obj); + physics::ModelPtr model=world->ModelByName(id); #else - physics::ModelPtr model=world->GetModel(obj); + physics::ModelPtr model=world->GetModel(id); #endif if (model) { - world->RemoveModel(obj); - objects.erase(it); - return true; + world->RemoveModel(id); + objects.erase(id); + return true; } else { - it++; + yError()<<"WorldProxy::deleteObject: the obj called" << id << "not exists"; + return false; } - } - return false; } bool WorldProxy::deleteAll() From ba78bfa94298b74c8d6dffcc04d44199052401c8 Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Thu, 6 Sep 2018 11:32:00 +0200 Subject: [PATCH 14/25] Developed Double laser plugin --- plugins/CMakeLists.txt | 1 + plugins/doublelaser/CMakeLists.txt | 29 ++ .../doublelaser/include/gazebo/DoubleLaser.hh | 70 +++++ plugins/doublelaser/src/DoubleLaser.cc | 267 ++++++++++++++++++ 4 files changed, 367 insertions(+) create mode 100644 plugins/doublelaser/CMakeLists.txt create mode 100644 plugins/doublelaser/include/gazebo/DoubleLaser.hh create mode 100644 plugins/doublelaser/src/DoubleLaser.cc diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt index 838882c25..cd156ffe2 100644 --- a/plugins/CMakeLists.txt +++ b/plugins/CMakeLists.txt @@ -30,6 +30,7 @@ endif() add_subdirectory(worldinterface) add_subdirectory(linkattacher) add_subdirectory(lasersensor) +add_subdirectory(doublelaser) add_subdirectory(depthCamera) diff --git a/plugins/doublelaser/CMakeLists.txt b/plugins/doublelaser/CMakeLists.txt new file mode 100644 index 000000000..7a193a221 --- /dev/null +++ b/plugins/doublelaser/CMakeLists.txt @@ -0,0 +1,29 @@ +# Copyright (C) 2007-2015 Istituto Italiano di Tecnologia ADVR & iCub Facility & RBCS Department +# Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano +# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + +cmake_minimum_required(VERSION 2.8.7) + +PROJECT(Plugin_DoubleLaser) + +message("sono nel doublelaser!!") + +include(AddGazeboYarpPluginTarget) + +set(doubleLaser_source src/DoubleLaser.cc ) + +set(doubleLaser_headers include/gazebo/DoubleLaser.hh ) + + +set(LIB_COMMON_NAME gazebo_yarp_lib_common) +if(CMAKE_VERSION VERSION_LESS 3.0.0) + get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) + unset(LIB_COMMON_NAME) +endif() + +add_gazebo_yarp_plugin_target(LIBRARY_NAME doublelaser + INCLUDE_DIRS include/gazebo include/yarp/dev + SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} + LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} RayPlugin ${Boost_LIBRARIES} + HEADERS ${doubleLaser_headers} + SOURCES ${doubleLaser_source}) diff --git a/plugins/doublelaser/include/gazebo/DoubleLaser.hh b/plugins/doublelaser/include/gazebo/DoubleLaser.hh new file mode 100644 index 000000000..75daf33cc --- /dev/null +++ b/plugins/doublelaser/include/gazebo/DoubleLaser.hh @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR + * Authors: see AUTHORS file. + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +/** + * @file DoubleLaser.h + * @authors: Valentina Gaggero + */ + +#ifndef GAZEBOYARP_DOUBLELASER_HH +#define GAZEBOYARP_DOUBLELASER_HH + +#include + +#include + +#include + +namespace yarp { + namespace dev { + class IMultipleWrapper; + } +} + +namespace gazebo +{ +/// \class GazeboYarpDoubleLaser +/// Gazebo Plugin emulating the yarp double laser device in Gazebo. +///. +/// It can be configurated using the yarpConfigurationFile sdf tag, +/// that contains a Gazebo URI pointing at a yarp .ini configuration file +/// containing the configuration parameters of the doublelaser +/// + + +class GazeboYarpDoubleLaser : public ModelPlugin +{ +public: + GazeboYarpDoubleLaser(); + virtual ~GazeboYarpDoubleLaser(); + + /** + * Saves the gazebo pointer, creates the device driver + */ + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + +private: + + bool readConfigurationFromFile(physics::ModelPtr _parent, sdf::ElementPtr _sdf); //Getting .ini configuration file from sdf + yarp::dev::PolyDriver m_wrapper_rangeFinder; + yarp::dev::IMultipleWrapper* m_iWrap_rangeFinder; + + yarp::dev::PolyDriver m_driver_doublelaser; + yarp::dev::IMultipleWrapper* m_iWrap_doublelaser; + + yarp::dev::PolyDriverList m_lasers; //contains pointers of front and back laser + + yarp::os::Property m_parameters; + + std::string m_sensorName; + + yarp::dev::PolyDriver * m_driver_laserFront; + yarp::dev::PolyDriver * m_driver_laserBack; +}; + +} + +#endif diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc new file mode 100644 index 000000000..cd5130a29 --- /dev/null +++ b/plugins/doublelaser/src/DoubleLaser.cc @@ -0,0 +1,267 @@ +/* + * Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR + * Authors: see AUTHORS file. + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +#include "DoubleLaser.hh" +#include +#include +#include + +#include + +#include +#include +#include +#include + + +/** + * @file DoubleLaser.h + * @authors: Valentina Gaggero + */ + +using namespace std; + +namespace gazebo +{ + +GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser) + + GazeboYarpDoubleLaser::GazeboYarpDoubleLaser() : m_iWrap_rangeFinder(0), m_iWrap_doublelaser(0) + {} + + GazeboYarpDoubleLaser::~GazeboYarpDoubleLaser() + { + if (m_iWrap_doublelaser) + { + m_iWrap_doublelaser->detachAll(); + m_iWrap_rangeFinder = 0; + } + + if (m_iWrap_rangeFinder) + { + m_iWrap_rangeFinder->detachAll(); + m_iWrap_rangeFinder = 0; + } + + if (m_wrapper_rangeFinder.isValid()) + { + m_wrapper_rangeFinder.close(); + } + + + for (int n = 0; n < m_lasers.size(); n++) { + std::string scopedDeviceName = m_sensorName + "::" + m_lasers[n]->key.c_str(); + GazeboYarpPlugins::Handler::getHandler()->removeDevice(scopedDeviceName); + } + + GazeboYarpPlugins::Handler::getHandler()->removeSensor(m_sensorName); + yarp::os::Network::fini(); + } + + + bool GazeboYarpDoubleLaser::readConfigurationFromFile(physics::ModelPtr _parent, sdf::ElementPtr _sdf) + { + if (!_sdf->HasElement("yarpConfigurationFile")) + { + yError() << "GazeboYarpDoubleLaser: error: unable to load configuration"; + return false; + } + + std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); + std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); + + if (ini_file_path == "") + { + yError() << "GazeboYarpDoubleLaser: ini file path is empty"; + return false; + } + + GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_parent,_sdf,m_parameters); + + bool wipe = false; //in order to not clear m_parameters + if (! m_parameters.fromConfigFile(ini_file_path.c_str(),wipe)) + { + yError() << "GazeboYarpDoubleLaser: error reading parameters from config file= " << ini_file_name << "in" <GetScopedName(); + + //3) load configuration from sdf + if(!readConfigurationFromFile( _parent, _sdf)) + return; + + // 4) Insert the pointer in the singleton handler for retriving it in the yarp driver + GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent)); + + + //5) open wrapper Rangefinder2DWrapper + yarp::os::Property wrapper_parameters; + if(!m_parameters.check("WRAPPER")) + { + yError() << "GazeboYarpDoubleLaser: [WRAPPER] group is missing in configuration file"; + return; + } + + wrapper_parameters.fromString(m_parameters.findGroup("WRAPPER").toString()); + if(m_parameters.check("ROS")) + { + wrapper_parameters.addGroup("ROS").fromString(m_parameters.findGroup("ROS").toString()); + } + else + { + yInfo() << "GazeboYarpDoubleLaser: ROS group is missing in configuration file"; + } + + if(! m_wrapper_rangeFinder.open(wrapper_parameters)) + { + yError()<<"GazeboYarpDoubleLaser failed: error in opening yarp driver wrapper Rangefinder2DWrapper!!!"; + return; + } + + if (!m_wrapper_rangeFinder.view(m_iWrap_rangeFinder)) + { + yError("GazeboYarpDoubleLaser : wrapper interface not found, load failed. Rangefinder2DWrapper"); + return; + } + + // 6) Open the driver DoubleLaser + yarp::os::Property doublelaser_dev_parameters; + if(!m_parameters.check("onSimulator")) + { + yError() << "GazeboYarpDoubleLaser: onSimulator parameter is missing in configuration file"; + return; + } + doublelaser_dev_parameters.put("onSimulator", m_parameters.find("onSimulator").asBool()); + + doublelaser_dev_parameters.put("device", "cerDoubleLidar"); + + if(!m_parameters.check("LASERFRONT-CFG")) + { + yError() << "GazeboYarpDoubleLaser: LASERFRONT-CFG group is missing in configuration file"; + return; + } + + doublelaser_dev_parameters.addGroup("LASERFRONT-CFG").fromString(m_parameters.findGroup("LASERFRONT-CFG").toString()); + + if(!m_parameters.check("LASERBACK-CFG")) + { + yError() << "GazeboYarpDoubleLaser: LASERBACK-CFG group is missing in configuration file"; + return; + } + doublelaser_dev_parameters.addGroup("LASERBACK-CFG").fromString(m_parameters.findGroup("LASERBACK-CFG").toString()); + + if(!m_driver_doublelaser.open(doublelaser_dev_parameters) ) + { + yError()<<"GazeboYarpDoubleLaser: error in opening yarp driver doubleLaser"; + return; + } + + // 7 )finds device of laser front and laser back. the device names are written in configuration .ini file + yarp::os::Bottle &front_name = m_parameters.findGroup("LASERFRONT-CFG").findGroup("sensorName"); + if(front_name.isNull()) + { + yError() << "GazeboYarpDoubleLaser: cannot find LASERFRONT-CFG.sensorName parameter"; + return; + } + yarp::os::Bottle &back_name = m_parameters.findGroup("LASERBACK-CFG").findGroup("sensorName"); + if(back_name.isNull()) + { + yError() << "GazeboYarpDoubleLaser: cannot find LASERBACK-CFG.sensorName parameter"; + return; + } + + + std::string laserFront_name = front_name.find("sensorName").asString(); + std::string laserBack_name = back_name.find("sensorName").asString(); + + + m_driver_laserFront = GazeboYarpPlugins::Handler::getHandler()->getDevice(laserFront_name); + if(m_driver_laserFront == nullptr) + { + yError() << "GazeboYarpDoubleLaser: cannot find laserFront device"; + return; + } + + m_driver_laserBack = GazeboYarpPlugins::Handler::getHandler()->getDevice(laserBack_name); + if(m_driver_laserBack == nullptr) + { + yError() << "GazeboYarpDoubleLaser: cannot find laserBack device"; + return; + } + + yarp::dev::PolyDriverList listoflasers; //it will contain front and back laser device pointers + yarp::dev::PolyDriverDescriptor laserFront_desc; + yarp::dev::PolyDriverDescriptor laserBack_desc; + + laserFront_desc.poly = m_driver_laserFront; + laserFront_desc.key = laserFront_name; + listoflasers.push(laserFront_desc); + + laserBack_desc.poly = m_driver_laserBack; + laserBack_desc.key = laserBack_name; + listoflasers.push(laserBack_desc); + + m_driver_doublelaser.view(m_iWrap_doublelaser); + if(!m_iWrap_doublelaser->attachAll(listoflasers)) + { + yError() << "GazeboYarpDoubleLaser: error douring attaching double laser to front and back laser devices"; + m_driver_doublelaser.close(); + return; + } + + + // 8 )attach rangefinder wrapper to double laser + yarp::dev::PolyDriverList listofdoubellaser; //it will contain only double laser + yarp::dev::PolyDriverDescriptor doublelaser_desc; + doublelaser_desc.poly = &m_driver_doublelaser; + + listofdoubellaser.push(doublelaser_desc); + + if(m_iWrap_rangeFinder->attachAll(listofdoubellaser)) + { + yError() << "GazeboYarpDoubleLaser: ho fatto attach di rangeFinder wrapper al double laser. OK"; + } + else + { + yError() << "GazeboYarpDoubleLaser: ERRORE mentre facevo attach di rangeFinder wrapper al double laser."; + } + + + //Insert the pointer in the singleton handler for retrieving it in the yarp driver + GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent)); + + } + +} From 02f7f47b9cd7d214d99ece20375ce7b1c4d1863a Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Mon, 1 Apr 2019 11:14:23 +0200 Subject: [PATCH 15/25] LaserSensor: added deviceId setting DoubleLaser uses this parameter to differentiate front and back lasers --- plugins/lasersensor/src/LaserSensor.cc | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/plugins/lasersensor/src/LaserSensor.cc b/plugins/lasersensor/src/LaserSensor.cc index 73445bfc6..eea5a2087 100644 --- a/plugins/lasersensor/src/LaserSensor.cc +++ b/plugins/lasersensor/src/LaserSensor.cc @@ -97,6 +97,19 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd return; } + if(!driver_properties.check("deviceId")) + { + yError()<<"GazeboYarpLaserSensor Plugin failed: cannot find deviceId parameter in ini file."; + return; + } + std::string deviceId = driver_properties.find("deviceId").asString(); + + if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver)) + { + yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")"; + return; + } + //Attach the driver to the wrapper ::yarp::dev::PolyDriverList driver_list; From acc2345d15101d4fe7b444186111f9f49dc41306 Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Mon, 1 Apr 2019 11:15:10 +0200 Subject: [PATCH 16/25] DoubleLaser: fix copyright and typos --- plugins/doublelaser/CMakeLists.txt | 15 ++++++------ .../doublelaser/include/gazebo/DoubleLaser.hh | 8 +++---- plugins/doublelaser/src/DoubleLaser.cc | 23 ++++++++----------- 3 files changed, 21 insertions(+), 25 deletions(-) diff --git a/plugins/doublelaser/CMakeLists.txt b/plugins/doublelaser/CMakeLists.txt index 7a193a221..8c7b78185 100644 --- a/plugins/doublelaser/CMakeLists.txt +++ b/plugins/doublelaser/CMakeLists.txt @@ -1,26 +1,25 @@ -# Copyright (C) 2007-2015 Istituto Italiano di Tecnologia ADVR & iCub Facility & RBCS Department -# Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano -# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT +#Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia - iCub Facility +#Authors: see AUTHORS file. +#CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + cmake_minimum_required(VERSION 2.8.7) PROJECT(Plugin_DoubleLaser) -message("sono nel doublelaser!!") - include(AddGazeboYarpPluginTarget) set(doubleLaser_source src/DoubleLaser.cc ) set(doubleLaser_headers include/gazebo/DoubleLaser.hh ) - - + set(LIB_COMMON_NAME gazebo_yarp_lib_common) + if(CMAKE_VERSION VERSION_LESS 3.0.0) get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) unset(LIB_COMMON_NAME) endif() - + add_gazebo_yarp_plugin_target(LIBRARY_NAME doublelaser INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} diff --git a/plugins/doublelaser/include/gazebo/DoubleLaser.hh b/plugins/doublelaser/include/gazebo/DoubleLaser.hh index 75daf33cc..ce8c37dcb 100644 --- a/plugins/doublelaser/include/gazebo/DoubleLaser.hh +++ b/plugins/doublelaser/include/gazebo/DoubleLaser.hh @@ -1,11 +1,11 @@ /* - * Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR + * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia - iCub Facility * Authors: see AUTHORS file. * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT */ /** - * @file DoubleLaser.h + * @file DoubleLaser.hh * @authors: Valentina Gaggero */ @@ -29,9 +29,9 @@ namespace gazebo /// \class GazeboYarpDoubleLaser /// Gazebo Plugin emulating the yarp double laser device in Gazebo. ///. -/// It can be configurated using the yarpConfigurationFile sdf tag, +/// It can be configured using the yarpConfigurationFile sdf tag, /// that contains a Gazebo URI pointing at a yarp .ini configuration file -/// containing the configuration parameters of the doublelaser +/// containing the configuration parameters of the DoubleLaser /// diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc index cd5130a29..6d4e9f476 100644 --- a/plugins/doublelaser/src/DoubleLaser.cc +++ b/plugins/doublelaser/src/DoubleLaser.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR + * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia - iCub Facility * Authors: see AUTHORS file. * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT */ @@ -18,7 +18,7 @@ /** - * @file DoubleLaser.h + * @file DoubleLaser.cc * @authors: Valentina Gaggero */ @@ -122,7 +122,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser) if(!readConfigurationFromFile( _parent, _sdf)) return; - // 4) Insert the pointer in the singleton handler for retriving it in the yarp driver + // 4) Insert the pointer in the singleton handler for retrieving it in the yarp driver GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent)); @@ -146,13 +146,13 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser) if(! m_wrapper_rangeFinder.open(wrapper_parameters)) { - yError()<<"GazeboYarpDoubleLaser failed: error in opening yarp driver wrapper Rangefinder2DWrapper!!!"; + yError()<<"GazeboYarpDoubleLaser failed: error opening yarp driver wrapper Rangefinder2DWrapper"; return; } if (!m_wrapper_rangeFinder.view(m_iWrap_rangeFinder)) { - yError("GazeboYarpDoubleLaser : wrapper interface not found, load failed. Rangefinder2DWrapper"); + yError("GazeboYarpDoubleLaser : wrapper (Rangefinder2DWrapper) interface not found, load failed."); return; } @@ -184,7 +184,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser) if(!m_driver_doublelaser.open(doublelaser_dev_parameters) ) { - yError()<<"GazeboYarpDoubleLaser: error in opening yarp driver doubleLaser"; + yError()<<"GazeboYarpDoubleLaser: error opening DoubleLaser yarp device "; return; } @@ -236,7 +236,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser) m_driver_doublelaser.view(m_iWrap_doublelaser); if(!m_iWrap_doublelaser->attachAll(listoflasers)) { - yError() << "GazeboYarpDoubleLaser: error douring attaching double laser to front and back laser devices"; + yError() << "GazeboYarpDoubleLaser: error attaching double laser to front and back laser devices"; m_driver_doublelaser.close(); return; } @@ -249,13 +249,10 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser) listofdoubellaser.push(doublelaser_desc); - if(m_iWrap_rangeFinder->attachAll(listofdoubellaser)) + if(!m_iWrap_rangeFinder->attachAll(listofdoubellaser)) { - yError() << "GazeboYarpDoubleLaser: ho fatto attach di rangeFinder wrapper al double laser. OK"; - } - else - { - yError() << "GazeboYarpDoubleLaser: ERRORE mentre facevo attach di rangeFinder wrapper al double laser."; + yError() << "GazeboYarpDoubleLaser: error attaching wrapper to double laser device"; + return; } From caa999ad617e4260e0bd0881b0a17ae103419a6e Mon Sep 17 00:00:00 2001 From: Luca Tricerri Date: Tue, 9 Apr 2019 23:55:17 +0200 Subject: [PATCH 17/25] Added Reset Simulation (#345) Co-Authored-By: triccyx --- .../GazeboYarpPlugins/ClockServerImpl.h | 3 +- plugins/clock/include/gazebo/Clock.hh | 5 ++ plugins/clock/src/Clock.cc | 5 ++ plugins/clock/src/ClockServerImpl.cpp | 5 ++ .../clock/autogenerated/include/ClockServer.h | 4 ++ .../clock/autogenerated/src/ClockServer.cpp | 50 +++++++++++++++++++ thrift/clock/clock_rpc.thrift | 5 ++ 7 files changed, 76 insertions(+), 1 deletion(-) diff --git a/plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h b/plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h index 78cbbc3ca..c35eb2813 100644 --- a/plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h +++ b/plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h @@ -23,7 +23,8 @@ class GazeboYarpPlugins::ClockServerImpl : public GazeboYarpPlugins::ClockServer virtual void continueSimulation(); virtual void stepSimulation(const int32_t numberOfSteps = 1); virtual void stepSimulationAndWait(const int32_t numberOfSteps = 1); - void resetSimulationTime(); + virtual void resetSimulationTime(); + virtual void resetSimulation(); virtual double getSimulationTime(); virtual double getStepSize(); diff --git a/plugins/clock/include/gazebo/Clock.hh b/plugins/clock/include/gazebo/Clock.hh index 9844ce7ae..e60b96052 100644 --- a/plugins/clock/include/gazebo/Clock.hh +++ b/plugins/clock/include/gazebo/Clock.hh @@ -100,6 +100,11 @@ public: * Reset the simulation time back to zero */ void resetSimulationTime(); + + /** + * Reset the simulation time and state back to zero + */ + void resetSimulation(); /** * Get the current step size in seconds. diff --git a/plugins/clock/src/Clock.cc b/plugins/clock/src/Clock.cc index 08cf97375..62cff254f 100644 --- a/plugins/clock/src/Clock.cc +++ b/plugins/clock/src/Clock.cc @@ -177,6 +177,11 @@ namespace gazebo m_world->ResetTime(); } + void GazeboYarpClock::resetSimulation() + { + m_world->Reset(); + } + common::Time GazeboYarpClock::getSimulationTime() { #if GAZEBO_MAJOR_VERSION >= 8 diff --git a/plugins/clock/src/ClockServerImpl.cpp b/plugins/clock/src/ClockServerImpl.cpp index 1c8408d38..2b0373f1e 100644 --- a/plugins/clock/src/ClockServerImpl.cpp +++ b/plugins/clock/src/ClockServerImpl.cpp @@ -37,6 +37,11 @@ namespace GazeboYarpPlugins { { m_clock.resetSimulationTime(); } + + void ClockServerImpl::resetSimulation() + { + m_clock.resetSimulation(); + } double ClockServerImpl::getSimulationTime() { diff --git a/thrift/clock/autogenerated/include/ClockServer.h b/thrift/clock/autogenerated/include/ClockServer.h index 3d66ab75b..2aab41ecf 100644 --- a/thrift/clock/autogenerated/include/ClockServer.h +++ b/thrift/clock/autogenerated/include/ClockServer.h @@ -60,6 +60,10 @@ class GazeboYarpPlugins::ClockServer : public yarp::os::Wire { * @return the step size in seconds */ virtual double getStepSize(); + /** + * Reset the simulation state and time + */ + virtual void resetSimulation(); virtual bool read(yarp::os::ConnectionReader& connection) override; virtual std::vector help(const std::string& functionName="--all"); }; diff --git a/thrift/clock/autogenerated/src/ClockServer.cpp b/thrift/clock/autogenerated/src/ClockServer.cpp index ee0414451..cdbad17e8 100644 --- a/thrift/clock/autogenerated/src/ClockServer.cpp +++ b/thrift/clock/autogenerated/src/ClockServer.cpp @@ -68,6 +68,13 @@ class ClockServer_getStepSize : public yarp::os::Portable { virtual bool read(yarp::os::ConnectionReader& connection) override; }; +class ClockServer_resetSimulation : public yarp::os::Portable { +public: + void init(); + virtual bool write(yarp::os::ConnectionWriter& connection) const override; + virtual bool read(yarp::os::ConnectionReader& connection) override; +}; + bool ClockServer_pauseSimulation::write(yarp::os::ConnectionWriter& connection) const { yarp::os::idl::WireWriter writer(connection); if (!writer.writeListHeader(1)) return false; @@ -190,6 +197,21 @@ void ClockServer_getStepSize::init() { _return = (double)0; } +bool ClockServer_resetSimulation::write(yarp::os::ConnectionWriter& connection) const { + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) return false; + if (!writer.writeTag("resetSimulation",1,1)) return false; + return true; +} + +bool ClockServer_resetSimulation::read(yarp::os::ConnectionReader& connection) { + YARP_UNUSED(connection); + return true; +} + +void ClockServer_resetSimulation::init() { +} + ClockServer::ClockServer() { yarp().setOwner(*this); } @@ -253,6 +275,14 @@ double ClockServer::getStepSize() { bool ok = yarp().write(helper,helper); return ok?helper._return:_return; } +void ClockServer::resetSimulation() { + ClockServer_resetSimulation helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?","void ClockServer::resetSimulation()"); + } + yarp().write(helper); +} bool ClockServer::read(yarp::os::ConnectionReader& connection) { yarp::os::idl::WireReader reader(connection); @@ -362,6 +392,21 @@ bool ClockServer::read(yarp::os::ConnectionReader& connection) { reader.accept(); return true; } + if (tag == "resetSimulation") { + if (!direct) { + ClockServer_resetSimulation helper; + helper.init(); + yarp().callback(helper,*this,"__direct__"); + } else { + resetSimulation(); + } + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeOnewayResponse()) return false; + } + reader.accept(); + return true; + } if (tag == "help") { std::string functionName; if (!reader.readString(functionName)) { @@ -403,6 +448,7 @@ std::vector ClockServer::help(const std::string& functionName) { helpString.push_back("resetSimulationTime"); helpString.push_back("getSimulationTime"); helpString.push_back("getStepSize"); + helpString.push_back("resetSimulation"); helpString.push_back("help"); } else { @@ -443,6 +489,10 @@ std::vector ClockServer::help(const std::string& functionName) { helpString.push_back("Get the current step size in seconds. "); helpString.push_back("@return the step size in seconds "); } + if (functionName=="resetSimulation") { + helpString.push_back("void resetSimulation() "); + helpString.push_back("Reset the simulation state and time "); + } if (functionName=="help") { helpString.push_back("std::vector help(const std::string& functionName=\"--all\")"); helpString.push_back("Return list of available commands, or help message for a specific function"); diff --git a/thrift/clock/clock_rpc.thrift b/thrift/clock/clock_rpc.thrift index 6c3f00aaa..77ab2924f 100644 --- a/thrift/clock/clock_rpc.thrift +++ b/thrift/clock/clock_rpc.thrift @@ -41,5 +41,10 @@ service ClockServer { * @return the step size in seconds */ double getStepSize(); + + /** Reset the simulation state and time + * + */ + oneway void resetSimulation(); } From 37eff49eda96bea0a38dc800fd788cd6b433fd1e Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Tue, 16 Apr 2019 09:33:44 +0200 Subject: [PATCH 18/25] Update external wrench plugin with mutiple wrenches and improve rpc responses (#418) --- plugins/externalwrench/CMakeLists.txt | 4 +- plugins/externalwrench/README.md | 31 ++ .../include/gazebo/ApplyExternalWrench.hh | 60 +-- .../include/gazebo/ExternalWrench.hh | 88 ++++ .../externalwrench/src/ApplyExternalWrench.cc | 386 ++++++++---------- plugins/externalwrench/src/ExternalWrench.cc | 201 +++++++++ 6 files changed, 518 insertions(+), 252 deletions(-) create mode 100644 plugins/externalwrench/README.md create mode 100644 plugins/externalwrench/include/gazebo/ExternalWrench.hh create mode 100644 plugins/externalwrench/src/ExternalWrench.cc diff --git a/plugins/externalwrench/CMakeLists.txt b/plugins/externalwrench/CMakeLists.txt index b27b02635..c47e71f9b 100644 --- a/plugins/externalwrench/CMakeLists.txt +++ b/plugins/externalwrench/CMakeLists.txt @@ -12,8 +12,8 @@ add_gazebo_yarp_plugin_target(LIBRARY_NAME externalwrench INCLUDE_DIRS include/gazebo SYSTEM_INCLUDE_DIRS ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} LINKED_LIBRARIES ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} - HEADERS include/gazebo/ApplyExternalWrench.hh - SOURCES src/ApplyExternalWrench.cc + HEADERS include/gazebo/ApplyExternalWrench.hh include/gazebo/ExternalWrench.hh + SOURCES src/ApplyExternalWrench.cc src/ExternalWrench.cc ) diff --git a/plugins/externalwrench/README.md b/plugins/externalwrench/README.md new file mode 100644 index 000000000..fc9606168 --- /dev/null +++ b/plugins/externalwrench/README.md @@ -0,0 +1,31 @@ +### ExternalWrench Plugin + +The ExternalWrench is a Gazebo _model_plugin_. It is used to apply external wrenches on any of the links of the model (to which this plugin is added). + +### Usage +The plugin takes `gazebo_icub_robotname.ini` as the configuration file. Add the following lines to the sdf model. + + +``` + + model://icub/conf/gazebo_icub_robotname.ini + +``` + +On launching the model in Gazebo, the _rpc_ port will be opened e.g. `//applyExternalWrench/rpc:i`. Connect to this port and type `help` to know the available commands. + +``` +Responses: + The defaul operation mode is with single wrench + Insert [single] or [multiple] to change the operation mode + Insert a command with the following format: + [link] [force] [torque] [duration] + e.g. chest 10 0 0 0 0 0 1 + [link]: (string) Link ID of the robot as specified in robot's SDF + [force]: (double x, y, z) Force components in N w.r.t. world reference frame + [torque]: (double x, y, z) Torque components in N.m w.r.t world reference frame + [duration]: (double) Duration of the applied force in seconds + Note: The reference frame is the base/root robot frame with x pointing backwards and z upwards. +``` + +The plugin will operate in _single_ wrench operation mode by default. Users can switch to _multiple_ wrenches operation mode. To change the operation mode of the plugin enter `single` or `multiple` in the rpc terminal. Every time the operation mode is changed the wrenches present at the moment will be cleared. To apply external wrenches enter the command format `[link] [force] [torque] [duration]` e.g. `l_hand 10 0 0 0 0 0 10` which applies an external wrench of pure force with 10 N magnitude for a duration of 10 seconds. Resetting Gazebo world will clear all the existing wrenches and change the operation mode of the plugin to default option i.e. `single`. diff --git a/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh b/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh index da47e2552..584d8d056 100644 --- a/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh +++ b/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh @@ -1,5 +1,5 @@ -#ifndef APPLYEXTERNALWRENCH_HH -#define APPLYEXTERNALWRENCH_HH +#ifndef GAZEBO_YARP_PLUGINS_APPLYEXTERNALWRENCH_HH +#define GAZEBO_YARP_PLUGINS_APPLYEXTERNALWRENCH_HH #include #include @@ -8,41 +8,55 @@ #include #include #include +#include #include #include #include #include #include -#include #include +#include +#include +#include "ExternalWrench.hh" // A YARP Thread class that will be used to read the rpc port to apply external wrench class RPCServerThread: public yarp::os::Thread { public: + // variable for operation mode + // single wrench or multiple wrenches + std::string m_mode; + + int wrenchCount; + virtual bool threadInit(); virtual void run(); virtual void threadRelease(); yarp::os::Bottle getCmd(); void setRobotName(std::string robotName); - void setScopedName(std::string scopedName); - void setDefaultLink(const std::string& defaultLink); - void setDurationBuffer(double d); - double getDurationBuffer(); + void setRobotModel(physics::ModelPtr robotModel); void setNewCommandFlag(std::int32_t flag); + void setLastTimeStamp(double& time); + yarp::os::Stamp getLastTimeStamp(); virtual void onStop(); + + std::vector wrenchesVector; + private: yarp::os::RpcServer m_rpcPort; yarp::os::Bottle m_cmd; yarp::os::Bottle m_reply; /// \brief Mutex to lock reading and writing of _cmd boost::mutex m_lock; + physics::ModelPtr m_robotModel; std::string m_robotName; - std::string m_scopedName; - std::string m_defaultLink; double m_durationBuffer; + + std::string m_message; + + yarp::os::Stamp m_lastTimestamp; }; @@ -56,26 +70,17 @@ private: public: ApplyExternalWrench(); virtual ~ApplyExternalWrench(); - std::string retrieveSubscope(gazebo::physics::Link_V& v, std::string scope); + void onUpdate(const gazebo::common::UpdateInfo&); + void onReset(); - struct wrench { - yarp::sig::Vector force; - yarp::sig::Vector torque; - double duration; - }; + bool getCandidateLink(physics::LinkPtr& candidateLink, std::string candidateLinkName); /// \brief Robot name that will be used to open rpc port std::string robotName; - double timeIni; -// yarp::os::Bottle bufferBottle; - protected: // Inherited void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ); - // Inherited - virtual void UpdateChild(); - private: yarp::os::Network m_yarpNet; @@ -83,14 +88,9 @@ private: yarp::os::Property m_iniParams; physics::ModelPtr m_myModel; - /// \brief Link on which the wrench will be applied - std::string m_modelScope; - std::string m_subscope; - std::string m_linkName; + /// \brief Link the plugin is attached to physics::LinkPtr m_onLink; - /// \brief Wrench to be applied on the body - wrench m_wrenchToApply; /// \brief Mutex to lock access boost::mutex m_lock; @@ -98,15 +98,17 @@ private: /// \brief Pointer to the update event connection event::ConnectionPtr m_updateConnection; + /// \brief Pointer to WorldReset Gazebo event + event::ConnectionPtr m_resetConnection; + transport::PublisherPtr m_visPub; msgs::Visual m_visualMsg; bool m_newCommand; - }; } -#endif +#endif //GAZEBO_YARP_PLUGINS_APPLYEXTERNALWRENCH_HH diff --git a/plugins/externalwrench/include/gazebo/ExternalWrench.hh b/plugins/externalwrench/include/gazebo/ExternalWrench.hh new file mode 100644 index 000000000..813ff8ef0 --- /dev/null +++ b/plugins/externalwrench/include/gazebo/ExternalWrench.hh @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + */ + +#ifndef GAZEBO_YARP_PLUGINS_EXTERNALWRENCH_H +#define GAZEBO_YARP_PLUGINS_EXTERNALWRENCH_H + +#include +#include +#include +#include +#include +#include + +#include "gazebo/gazebo.hh" +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gazebo; + +class ExternalWrench +{ +private: + + float color[4]; + struct wrenchCommand + { + std::string link_name; + yarp::sig::Vector force; + yarp::sig::Vector torque; + double duration; + } wrench; + + double tick; + double tock; + + int wrenchIndex; + + physics::ModelPtr model; + physics::LinkPtr link; + + transport::NodePtr node; + transport::PublisherPtr visPub; + msgs::Visual visualMsg; + + event::ConnectionPtr updateConnection; + +public: + + bool duration_done; + + ExternalWrench(); + ~ExternalWrench(); + + bool setWrench(physics::ModelPtr&, yarp::os::Bottle&); + bool getLink(); + + void setWrenchIndex(int& index); + void setWrenchColor(); + void setTick(double& tickTime); + void setTock(double& tockTime); + void setVisual(); + void applyWrench(); + void deleteWrench(); + void setModel(); +}; + +#endif //GAZEBO_YARP_PLUGINS_EXTERNALWRENCH_H diff --git a/plugins/externalwrench/src/ApplyExternalWrench.cc b/plugins/externalwrench/src/ApplyExternalWrench.cc index dad35ad12..7919995cf 100644 --- a/plugins/externalwrench/src/ApplyExternalWrench.cc +++ b/plugins/externalwrench/src/ApplyExternalWrench.cc @@ -13,150 +13,22 @@ GZ_REGISTER_MODEL_PLUGIN ( ApplyExternalWrench ) ApplyExternalWrench::ApplyExternalWrench() { - this->m_wrenchToApply.force.resize ( 3,0 ); - this->m_wrenchToApply.torque.resize ( 3,0 ); - this->m_wrenchToApply.duration = 0.0; - timeIni = 0; - m_newCommand = false; } + ApplyExternalWrench::~ApplyExternalWrench() { m_rpcThread.stop(); this->m_updateConnection.reset(); } -void ApplyExternalWrench::UpdateChild() -{ - // Reading apply wrench command - yarp::os::Bottle tmpBottle; - - // Copying command - this->m_lock.lock(); - tmpBottle = this->m_rpcThread.getCmd(); - if ( tmpBottle.get( 8 ).asInt32() == 1 ) - { - // If this is a new command - m_newCommand = true; - this->m_rpcThread.setNewCommandFlag( 0 ); - } else { - m_newCommand = false; - } - this->m_lock.unlock(); - - // initialCmdBottle will contain the initial bottle in RPCServerThread - static yarp::os::Bottle prevCmdBottle = tmpBottle; - - // Parsing command - this->m_linkName = tmpBottle.get ( 0 ).asString(); - for ( int i=1; i<4; i++ ) { - m_wrenchToApply.force[i-1] = tmpBottle.get ( i ).asDouble(); - } - for ( int i=4; i<7; i++ ) { - m_wrenchToApply.torque[i-4] = tmpBottle.get ( i ).asDouble(); - } - - std::string fullScopeLinkName = ""; - if(this->m_subscope!="") { - fullScopeLinkName = std::string ( this->m_modelScope + "::" + this->m_subscope + "::" + this->m_linkName ); - this->m_onLink = m_myModel->GetLink ( fullScopeLinkName ); - } else { - this->m_onLink = m_myModel->GetLink ( this->m_linkName ); - } - -// This piece of code shows the full name (scoped name + link name) of every link in the current loaded model. -// gazebo::physics::Link_V tmpLinksVector; -// tmpLinksVector = m_myModel->GetLinks(); -// for (int i=0; iGetName() << " "; -// yDebug() << std::endl; - - if ( !this->m_onLink ) { - //yError() << "ApplyWrench plugin: link named " << this->m_linkName<< " not found"; - return; - } - - // Get wrench duration - this->m_wrenchToApply.duration = tmpBottle.get ( 7 ).asDouble(); - - - // Taking duration of the applied force into account - static bool applying_force_flag = 0; - - double time_current = yarp::os::Time::now(); - - if ( m_newCommand ) - { - timeIni = yarp::os::Time::now(); - applying_force_flag = 1; - } - - // This has to be done during the specified duration - if ( applying_force_flag && ( time_current - timeIni ) < m_wrenchToApply.duration ) { -#if GAZEBO_MAJOR_VERSION >= 8 - // Copying command to force and torque Vector3 variables - ignition::math::Vector3d force ( this->m_wrenchToApply.force[0], this->m_wrenchToApply.force[1], this->m_wrenchToApply.force[2] ); - ignition::math::Vector3d torque ( this->m_wrenchToApply.torque[0], this->m_wrenchToApply.torque[1], this->m_wrenchToApply.torque[2] ); - this->m_onLink->AddForce ( force ); - this->m_onLink->AddTorque ( torque ); - ignition::math::Vector3d linkCoGPos = this->m_onLink->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied - ignition::math::Vector3d newZ = force.Normalize(); // normalized force. I want the z axis of the cylinder's reference frame to coincide with my force vector - ignition::math::Vector3d newX = newZ.Cross ( ignition::math::Vector3d::UnitZ ); - ignition::math::Vector3d newY = newZ.Cross ( newX ); - ignition::math::Matrix4d rotation = ignition::math::Matrix4d ( newX[0],newY[0],newZ[0],0,newX[1],newY[1],newZ[1],0,newX[2],newY[2],newZ[2],0, 0, 0, 0, 1 ); - ignition::math::Quaterniond forceOrientation = rotation.Rotation(); - ignition::math::Pose3d linkCoGPose ( linkCoGPos - rotation*ignition::math::Vector3d ( 0,0,.15 ), forceOrientation ); -#else - // Copying command to force and torque Vector3 variables - math::Vector3 force ( this->m_wrenchToApply.force[0], this->m_wrenchToApply.force[1], this->m_wrenchToApply.force[2] ); - math::Vector3 torque ( this->m_wrenchToApply.torque[0], this->m_wrenchToApply.torque[1], this->m_wrenchToApply.torque[2] ); - this->m_onLink->AddForce ( force ); - this->m_onLink->AddTorque ( torque ); - math::Vector3 linkCoGPos = this->m_onLink->GetWorldCoGPose().pos; // Get link's COG position where wrench will be applied - math::Vector3 newZ = force.Normalize(); // normalized force. I want the z axis of the cylinder's reference frame to coincide with my force vector - math::Vector3 newX = newZ.Cross ( math::Vector3::UnitZ ); - math::Vector3 newY = newZ.Cross ( newX ); - math::Matrix4 rotation = math::Matrix4 ( newX[0],newY[0],newZ[0],0,newX[1],newY[1],newZ[1],0,newX[2],newY[2],newZ[2],0, 0, 0, 0, 1 ); - math::Quaternion forceOrientation = rotation.GetRotation(); - math::Pose linkCoGPose ( linkCoGPos - rotation*math::Vector3 ( 0,0,.15 ), forceOrientation ); -#endif -#if GAZEBO_MAJOR_VERSION == 7 - msgs::Set ( m_visualMsg.mutable_pose(), linkCoGPose.Ign() ); -#else - msgs::Set ( m_visualMsg.mutable_pose(), linkCoGPose ); -#endif - double red = 1; - double alpha = 0.3; -#if GAZEBO_MAJOR_VERSION >= 9 - msgs::Set(m_visualMsg.mutable_material()->mutable_ambient(), ignition::math::Color(red,0,0,alpha)); -#else - msgs::Set(m_visualMsg.mutable_material()->mutable_ambient(), common::Color(red,0,0,alpha)); -#endif - m_visualMsg.set_visible ( 1 ); - m_visPub->Publish ( m_visualMsg ); - } - - if ( applying_force_flag && ( time_current - timeIni ) > m_wrenchToApply.duration ) { - applying_force_flag = 0; - m_visualMsg.set_visible ( 0 ); - m_visPub->Publish ( m_visualMsg ); - } - -} - void ApplyExternalWrench::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) { - // Check if yarp network is active; + // Check if yarp network is active if ( !this->m_yarpNet.checkNetwork() ) { yError ( "ERROR Yarp Network was not found active in ApplyExternalWrench plugin" ); return; } - // What is the parent name?? - this->m_modelScope = _model->GetScopedName(); - - // Copy the pointer to the model to access later from UpdateChild - this->m_myModel = _model; - bool configuration_loaded = false; // Read robot name @@ -168,11 +40,7 @@ void ApplyExternalWrench::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf yarp::os::Value robotNameParam = m_iniParams.find ( "gazeboYarpPluginsRobotName" ); this->robotName = robotNameParam.asString(); m_rpcThread.setRobotName ( robotName ); - m_rpcThread.setScopedName ( this->m_modelScope ); - gazebo::physics::Link_V links = _model->GetLinks(); - std::string defaultLink = links[0]->GetName(); - m_rpcThread.setDefaultLink(defaultLink); - this->m_subscope = retrieveSubscope(links, m_modelScope); + m_rpcThread.setRobotModel(_model); configuration_loaded = true; } else { yError ( "ERROR trying to get robot configuration file" ); @@ -181,9 +49,8 @@ void ApplyExternalWrench::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf } else { this->robotName = _model->GetName(); m_rpcThread.setRobotName ( robotName ); - m_rpcThread.setScopedName ( this->m_modelScope ); + m_rpcThread.setRobotModel(_model); gazebo::physics::Link_V links = _model->GetLinks(); - m_rpcThread.setDefaultLink(links.at(0)->GetName()); configuration_loaded = true; } @@ -192,85 +59,64 @@ void ApplyExternalWrench::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf yError ( "ERROR: rpcThread did not start correctly" ); } - - /// ############ TRYING TO MODIFY VISUALS ####################################################### - - - this->m_node = transport::NodePtr ( new gazebo::transport::Node() ); - -#if GAZEBO_MAJOR_VERSION >= 8 - std::string worldName = _model->GetWorld()->Name(); -#else - std::string worldName = _model->GetWorld()->GetName(); -#endif - - this->m_node->Init ( worldName ); - m_visPub = this->m_node->Advertise ( "~/visual", 10 ); - - // Set the visual's name. This should be unique. - m_visualMsg.set_name ( "__CYLINDER_VISUAL__" ); - - // Set the visual's parent. This visual will be attached to the parent - m_visualMsg.set_parent_name ( _model->GetScopedName() ); - - // Create a cylinder - msgs::Geometry *geomMsg = m_visualMsg.mutable_geometry(); - geomMsg->set_type ( msgs::Geometry::CYLINDER ); - geomMsg->mutable_cylinder()->set_radius ( 0.01 ); - geomMsg->mutable_cylinder()->set_length ( .30 ); - - // Don't cast shadows - m_visualMsg.set_cast_shadows ( false ); - - /// ############ END: TRYING TO MODIFY VISUALS ##################################################### - - // Listen to the update event. This event is broadcast every // simulation iteration. - this->m_updateConnection = event::Events::ConnectWorldUpdateBegin ( boost::bind ( &ApplyExternalWrench::UpdateChild, this ) ); -} + this->m_updateConnection = event::Events::ConnectWorldUpdateBegin ( boost::bind ( &ApplyExternalWrench::onUpdate, this, _1 ) ); -std::string ApplyExternalWrench::retrieveSubscope ( gazebo::physics::Link_V& v , std::string scope) -{ - std::string tmpName = v[0]->GetName(); - std::size_t found = tmpName.find_first_of(":"); - if(found!=std::string::npos) - tmpName = tmpName.substr (0, found); - else - tmpName = ""; - return tmpName; + // Listen to gazebo world reset event + this->m_resetConnection = event::Events::ConnectWorldReset ( boost::bind ( &ApplyExternalWrench::onReset, this ) ); } +void ApplyExternalWrench::onUpdate(const gazebo::common::UpdateInfo& _info) +{ + // Update last time stamp + double time = _info.simTime.Double(); + m_rpcThread.setLastTimeStamp(time); + this->m_lock.lock(); + for(int i = 0; i < m_rpcThread.wrenchesVector.size() ; i++) + { + // Update wrench tock time + yarp::os::Stamp tockTimeStamp = m_rpcThread.getLastTimeStamp(); + double tockTime = tockTimeStamp.getTime(); + m_rpcThread.wrenchesVector.at(i).setTock(tockTime); + + bool duration_check = m_rpcThread.wrenchesVector.at(i).duration_done; + if(duration_check == false) + { + m_rpcThread.wrenchesVector.at(i).applyWrench(); + } + } + this->m_lock.unlock(); } +void ApplyExternalWrench::onReset() +{ + this->m_lock.lock(); -// ############ RPCServerThread class ############### + // Delete all the wrenches + if (this->m_rpcThread.wrenchesVector.size() != 0) { + for (int i = 0; i < this->m_rpcThread.wrenchesVector.size(); i++) + { + ExternalWrench wrench = this->m_rpcThread.wrenchesVector.at(i); + wrench.deleteWrench(); + } + this->m_rpcThread.wrenchesVector.clear(); + } -void RPCServerThread::setRobotName ( std::string robotName ) -{ - this->m_robotName = robotName; -} + // Change the operation mode to default option 'single' + this->m_rpcThread.m_mode = "single"; -void RPCServerThread::setScopedName ( std::string scopedName ) -{ - this->m_scopedName = scopedName; -} + // Reset wrench count + this->m_rpcThread.wrenchCount = 0; -void RPCServerThread::setDurationBuffer ( double d ) -{ - this->m_durationBuffer = d; + this->m_lock.unlock(); } -double RPCServerThread::getDurationBuffer() -{ - return this->m_durationBuffer; } -void RPCServerThread::setDefaultLink(const std::string &defaultLink) -{ - this->m_defaultLink = defaultLink; -} + +// ############ RPCServerThread class ############### bool RPCServerThread::threadInit() { @@ -279,21 +125,16 @@ bool RPCServerThread::threadInit() yError ( "ERROR opening RPC port /applyExternalWrench" ); return false; } - // Default link on which wrenches are applied - //m_cmd.addString ( this->m_scopedName + "::l_arm" ); - m_cmd.addString ( this->m_defaultLink ); - m_cmd.addFloat64 ( 0 ); // Force coord. x - m_cmd.addFloat64 ( 0 ); // Force coord. y - m_cmd.addFloat64 ( 0 ); // Force coord. z - m_cmd.addFloat64 ( 0 ); // Torque coord. x - m_cmd.addFloat64 ( 0 ); // Torque coord. y - m_cmd.addFloat64 ( 0 ); // Torque coord. z - m_cmd.addFloat64 ( 0 ); // Wrench duration - - this->m_durationBuffer = m_cmd.get ( 7 ).asDouble(); + + // Set the default operation mode + this->m_mode = "single"; + + // Set wrench count default value + this->wrenchCount = 0; return true; } + void RPCServerThread::run() { while ( !isStopping() ) { @@ -301,6 +142,8 @@ void RPCServerThread::run() m_rpcPort.read ( command,true ); if ( command.get ( 0 ).asString() == "help" ) { this->m_reply.addVocab ( yarp::os::Vocab::encode ( "many" ) ); + this->m_reply.addString ( "The defaul operation mode is with single wrench" ); + this->m_reply.addString ( "Insert [single] or [multiple] to change the operation mode" ); this->m_reply.addString ( "Insert a command with the following format:" ); this->m_reply.addString ( "[link] [force] [torque] [duration]" ); this->m_reply.addString ( "e.g. chest 10 0 0 0 0 0 1"); @@ -310,21 +153,95 @@ void RPCServerThread::run() this->m_reply.addString ( "[duration]: (double) Duration of the applied force in seconds" ); this->m_reply.addString ( "Note: The reference frame is the base/root robot frame with x pointing backwards and z upwards."); this->m_rpcPort.reply ( this->m_reply ); - } else { - if ( command.get ( 0 ).isString() \ - && ( command.get ( 1 ).isDouble() || command.get ( 1 ).isInt() ) && ( command.get ( 2 ).isDouble() || command.get ( 2 ).isInt() ) && ( command.get ( 3 ).isDouble() || command.get ( 3 ).isInt() ) \ - && ( command.get ( 4 ).isDouble() || command.get ( 4 ).isInt() ) && ( command.get ( 5 ).isDouble() || command.get ( 5 ).isInt() ) && ( command.get ( 6 ).isDouble() || command.get ( 6 ).isInt() ) \ - && ( command.get ( 7 ).isDouble() || command.get ( 7 ).isInt() ) ) { - this->m_reply.addString ( "[ACK] Correct command format" ); - this->m_rpcPort.reply ( m_reply ); + } else{ + if((command.size() == 8) && (command.get(0).isString() \ + && ( command.get ( 1 ).isDouble() || command.get ( 1 ).isInt() ) && ( command.get ( 2 ).isDouble() || command.get ( 2 ).isInt() ) && ( command.get ( 3 ).isDouble() || command.get ( 3 ).isInt() ) \ + && ( command.get ( 4 ).isDouble() || command.get ( 4 ).isInt() ) && ( command.get ( 5 ).isDouble() || command.get ( 5 ).isInt() ) && ( command.get ( 6 ).isDouble() || command.get ( 6 ).isInt() ) \ + && ( command.get ( 7 ).isDouble() || command.get ( 7 ).isInt() )) ) { + this->m_message = "[ACK] Correct command format"; m_lock.lock(); // new-command flag command.addInt32(1); m_cmd = command; m_lock.unlock(); - } else { + + if (this->m_mode == "single") { + + // Reset wrench count + wrenchCount = 0; + + // Delete the previous wrenches + if (wrenchesVector.size() != 0) { + for (int i = 0; i < wrenchesVector.size(); i++) + { + ExternalWrench wrench = wrenchesVector.at(i); + wrench.deleteWrench(); + } + wrenchesVector.clear(); + } + + } + + // Create new instances of external wrenches + ExternalWrench newWrench; + if(newWrench.setWrench(m_robotModel, m_cmd)) + { + // Update wrench count + wrenchCount++; + + // Set wrench tick time + yarp::os::Stamp tickTimeStamp = this->getLastTimeStamp(); + double tickTime = tickTimeStamp.getTime(); + newWrench.setTick(tickTime); + + // Set wrench index + newWrench.setWrenchIndex(wrenchCount); + + // Set wrench color + newWrench.setWrenchColor(); + + // Set wrench visual + newWrench.setVisual(); + + this->m_message = this->m_message + " and " + command.get(0).asString() + " link found in the model" ; + this->m_reply.addString ( m_message); + this->m_rpcPort.reply ( m_reply ); + wrenchesVector.push_back(newWrench); + } + else + { this->m_message = this->m_message + " but " + command.get(0).asString() + " link found in the model" ; + this->m_reply.addString ( m_message ); + this->m_rpcPort.reply ( m_reply ); + } + } + else if (command.size() == 1 && command.get(0).isString()) { + + this->m_mode = command.get(0).asString(); + + this->m_message = command.get(0).asString() + " wrench operation mode set"; + + // Reset wrench count + wrenchCount = 0; + + // Delete the previous wrenches + if (wrenchesVector.size() != 0) { + this->m_message = this->m_message + " . Clearing previous wrenches."; + for (int i = 0; i < wrenchesVector.size(); i++) + { + ExternalWrench wrench = wrenchesVector.at(i); + wrench.deleteWrench(); + } + wrenchesVector.clear(); + } + + this->m_reply.addString (m_message); + this->m_rpcPort.reply ( m_reply ); + + } + else { this->m_reply.clear(); - this->m_reply.addString ( "ERROR: Incorrect command format" ); + this->m_message = "ERROR: Incorrect command format. Insert [help] to know the correct command format"; + this->m_reply.addString ( m_message ); this->m_rpcPort.reply ( this->m_reply ); } } @@ -332,6 +249,33 @@ void RPCServerThread::run() command.clear(); } } + +void RPCServerThread::setLastTimeStamp(double& time) +{ + this->m_lock.lock(); + + m_lastTimestamp.update(time); + + this->m_lock.unlock(); +} + +yarp::os::Stamp RPCServerThread::getLastTimeStamp() +{ + boost::lock_guard lock{this->m_lock}; + + return m_lastTimestamp; +} + +void RPCServerThread::setRobotModel(physics::ModelPtr robotModel) +{ + m_robotModel = robotModel; +} + +void RPCServerThread::setRobotName ( std::string robotName ) +{ + this->m_robotName = robotName; +} + void RPCServerThread::threadRelease() { yarp::os::Thread::threadRelease(); diff --git a/plugins/externalwrench/src/ExternalWrench.cc b/plugins/externalwrench/src/ExternalWrench.cc new file mode 100644 index 000000000..4d9b32ce5 --- /dev/null +++ b/plugins/externalwrench/src/ExternalWrench.cc @@ -0,0 +1,201 @@ +/* + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + */ + +#include "ExternalWrench.hh" +#include + +// Initializing wrench command +ExternalWrench::ExternalWrench() +{ + // Default wrench index + wrenchIndex = 0; + + // Default color + color[0] = 0; + color[1] = 0; + color[2] = 0; + color[3] = 0; + + // Default wrench values + wrench.link_name = "world"; + wrench.force.resize(3,0); + wrench.torque.resize(3,0); + wrench.duration = 0.0; + + tick = 0; // Default tick value + tock = 0; // Default tock value + duration_done = false; +} + +void ExternalWrench::setWrenchColor() +{ + // Set visual color + std::mt19937 rand_gen(this->wrenchIndex); //fixed seed based on wrench index + std::uniform_real_distribution<> uni_dis(0.0, 1.0); + + color[0] = uni_dis(rand_gen); + color[1] = uni_dis(rand_gen); + color[2] = uni_dis(rand_gen); + color[3] = uni_dis(rand_gen); +} + +void ExternalWrench::setWrenchIndex(int& index) +{ + this->wrenchIndex = index; +} + +void ExternalWrench::setTick(double& tickTime) +{ + this->tick = tickTime; +} + +void ExternalWrench::setTock(double& tockTime) +{ + this->tock = tockTime; +} + +bool ExternalWrench::getLink() +{ + // Get the exact link with only link name instead of full_scoped_link_name + physics::Link_V links = this->model->GetLinks(); + for(int i=0; i < links.size(); i++) + { + std::string candidate_link_name = links[i]->GetScopedName(); + + // hasEnding compare ending of the condidate model link name to the given link name, in order to be able to use unscoped names + if(GazeboYarpPlugins::hasEnding(candidate_link_name, wrench.link_name)) + { + link = links[i]; + break; + } + } + + if(!link) + { + return false; + } + + return true; +} + +void ExternalWrench::setVisual() +{ + // Wrench Visual + node = transport::NodePtr(new gazebo::transport::Node()); + this->node->Init(model->GetWorld()->Name()); + visPub = this->node->Advertise("~/visual",100); + + // Set the visual's name. This should be unique. + std::string visual_name = "GYP_EXT_WRENCH__" + wrench.link_name + "__CYLINDER_VISUAL__" + std::to_string(wrenchIndex); + visualMsg.set_name (visual_name); + + // Set the visual's parent. This visual will be attached to the parent + visualMsg.set_parent_name(model->GetScopedName()); + + // Create a cylinder + msgs::Geometry *geomMsg = visualMsg.mutable_geometry(); + geomMsg->set_type(msgs::Geometry::CYLINDER); + geomMsg->mutable_cylinder()->set_radius(0.0075); + geomMsg->mutable_cylinder()->set_length(.30); + + // Don't cast shadows + visualMsg.set_cast_shadows(false); +} + +bool ExternalWrench::setWrench(physics::ModelPtr& _model,yarp::os::Bottle& cmd) +{ + model = _model; + + // Get link name from command + wrench.link_name = cmd.get(0).asString(); + + if(getLink()) + { + wrench.force[0] = cmd.get(1).asDouble(); + wrench.force[1] = cmd.get(2).asDouble(); + wrench.force[2] = cmd.get(3).asDouble(); + + wrench.torque[0] = cmd.get(4).asDouble(); + wrench.torque[1] = cmd.get(5).asDouble(); + wrench.torque[2] = cmd.get(6).asDouble(); + + wrench.duration = cmd.get(7).asDouble(); + + return true; + } + else return false; +} + +void ExternalWrench::applyWrench() +{ + if((tock-tick) < wrench.duration) + { +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Vector3d force (wrench.force[0], wrench.force[1], wrench.force[2]); + ignition::math::Vector3d torque (wrench.torque[0], wrench.torque[1], wrench.torque[2]); + + link->AddForce(force); + link->AddTorque(torque); + + ignition::math::Vector3d linkCoGPos = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied + ignition::math::Vector3d newZ = force.Normalize(); // normalized force. I want the z axis of the cylinder's reference frame to coincide with my force vector + ignition::math::Vector3d newX = newZ.Cross (ignition::math::Vector3d::UnitZ); + ignition::math::Vector3d newY = newZ.Cross (newX); + ignition::math::Matrix4d rotation = ignition::math::Matrix4d (newX[0],newY[0],newZ[0],0,newX[1],newY[1],newZ[1],0,newX[2],newY[2],newZ[2],0, 0, 0, 0, 1); + ignition::math::Quaterniond forceOrientation = rotation.Rotation(); + ignition::math::Pose3d linkCoGPose (linkCoGPos - rotation*ignition::math::Vector3d ( 0,0,.15 ), forceOrientation); +#else + math::Vector3d force (wrench.force[0], wrench.force[1], wrench.force[2]); + math::Vector3d torque (wrench.torque[0], wrench.torque[1], wrench.torque[2]); + + link->AddForce(force); + link->AddTorque(torque); + + math::Vector3d linkCoGPos = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied + math::Vector3d newZ = force.Normalize(); // normalized force. I want the z axis of the cylinder's reference frame to coincide with my force vector + math::Vector3d newX = newZ.Cross (ignition::math::Vector3d::UnitZ); + math::Vector3d newY = newZ.Cross (newX); + math::Matrix4d rotation = ignition::math::Matrix4d (newX[0],newY[0],newZ[0],0,newX[1],newY[1],newZ[1],0,newX[2],newY[2],newZ[2],0, 0, 0, 0, 1); + math::Quaterniond forceOrientation = rotation.Rotation(); + math::Pose3d linkCoGPose (linkCoGPos - rotation*ignition::math::Vector3d ( 0,0,.15 ), forceOrientation); +#endif + +#if GAZEBO_MAJOR_VERSION == 7 + msgs::Set(visualMsg.mutable_pose(), linkCoGPose.Ign()); +#else + msgs::Set(visualMsg.mutable_pose(), linkCoGPose); +#endif +#if GAZEBO_MAJOR_VERSION >= 9 + msgs::Set(visualMsg.mutable_material()->mutable_ambient(), ignition::math::Color(color[0],color[1],color[2],color[3])); +#else + msgs::Set(visualMsg.mutable_material()->mutable_ambient(),common::Color(color[0],color[1],color[2],color[3])); +#endif + visualMsg.set_visible(1); + visPub->Publish(visualMsg); + } + else + { + deleteWrench(); + } +} + +void ExternalWrench::deleteWrench() +{ + this->wrench.link_name.clear(); + this->wrench.force.clear(); + this->wrench.torque.clear(); + this->wrench.duration = 0; + + this->visualMsg.set_visible(0); + this->visualMsg.clear_geometry(); + this->visualMsg.clear_delete_me(); + this->visPub->Publish(visualMsg); + this->duration_done = true; +} + +ExternalWrench::~ExternalWrench() +{} From 2a5dc3ca27a164f3353f8dd000306710d2ca2f96 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Thu, 18 Apr 2019 13:48:24 +0200 Subject: [PATCH 19/25] Update CMakeLists with gazebo_yarp_lib_common Fix #420 --- plugins/externalwrench/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/externalwrench/CMakeLists.txt b/plugins/externalwrench/CMakeLists.txt index c47e71f9b..485463984 100644 --- a/plugins/externalwrench/CMakeLists.txt +++ b/plugins/externalwrench/CMakeLists.txt @@ -11,7 +11,7 @@ include(AddGazeboYarpPluginTarget) add_gazebo_yarp_plugin_target(LIBRARY_NAME externalwrench INCLUDE_DIRS include/gazebo SYSTEM_INCLUDE_DIRS ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS include/gazebo/ApplyExternalWrench.hh include/gazebo/ExternalWrench.hh SOURCES src/ApplyExternalWrench.cc src/ExternalWrench.cc ) From 3ffd3a63b1c323744ff7a4aed27440a22fb9f4e8 Mon Sep 17 00:00:00 2001 From: Milad Shafiee <31568170+MiladShafiee@users.noreply.github.com> Date: Wed, 24 Apr 2019 13:50:19 +0200 Subject: [PATCH 20/25] Removing unused CMake checks for version less than 3.5 (#421) (#423) --- CMakeLists.txt | 4 ---- libraries/common/CMakeLists.txt | 7 ++----- libraries/singleton/CMakeLists.txt | 2 +- plugins/basestate/CMakeLists.txt | 8 +------- plugins/camera/CMakeLists.txt | 11 ++++------- plugins/clock/CMakeLists.txt | 9 ++------- plugins/configurationoverride/CMakeLists.txt | 5 ++--- plugins/contactloadcellarray/CMakeLists.txt | 8 +------- plugins/controlboard/CMakeLists.txt | 15 +-------------- plugins/depthCamera/CMakeLists.txt | 8 ++------ plugins/externalwrench/CMakeLists.txt | 1 - plugins/fakecontrolboard/CMakeLists.txt | 11 ++--------- plugins/forcetorque/CMakeLists.txt | 9 ++------- plugins/imu/CMakeLists.txt | 8 +------- plugins/inertialmtb/CMakeLists.txt | 9 ++------- plugins/inertialmtbPart/CMakeLists.txt | 8 +------- plugins/lasersensor/CMakeLists.txt | 8 +------- plugins/linkattacher/CMakeLists.txt | 3 +-- plugins/maissensor/CMakeLists.txt | 11 +++-------- plugins/modelposepublisher/CMakeLists.txt | 8 +------- plugins/multicamera/CMakeLists.txt | 8 +------- plugins/showmodelcom/CMakeLists.txt | 1 - plugins/videotexture/CMakeLists.txt | 9 ++------- plugins/worldinterface/CMakeLists.txt | 9 ++------- thrift/clock/CMakeLists.txt | 1 - thrift/worldinterface/CMakeLists.txt | 1 - 26 files changed, 35 insertions(+), 147 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b97f05de..0cf8a1233 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,10 +45,6 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake) #used for dir suffixes include(GNUInstallDirs) -if (CMAKE_VERSION LESS 3.0) - # hack: by default this would be 'lib/x86_64-linux-gnu' on linux - set(CMAKE_INSTALL_LIBDIR lib) -endif() OPTION(GAZEBO_YARP_PLUGINS_ENABLE_RPATH "Enable installation with RPATH" TRUE) include(AddInstallRPATHSupport) diff --git a/libraries/common/CMakeLists.txt b/libraries/common/CMakeLists.txt index 7cc87c4d3..e9d20218a 100644 --- a/libraries/common/CMakeLists.txt +++ b/libraries/common/CMakeLists.txt @@ -2,13 +2,11 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.11) + PROJECT(Library_Common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - set_property(GLOBAL APPEND PROPERTY GAZEBO_YARP_COMMON_HEADERS "${CMAKE_CURRENT_SOURCE_DIR}/include") -else() + add_library(gazebo_yarp_lib_common INTERFACE) target_include_directories(gazebo_yarp_lib_common INTERFACE $ @@ -18,4 +16,3 @@ else() install(TARGETS gazebo_yarp_lib_common EXPORT GazeboYARPPlugins) install(FILES include/GazeboYarpPlugins/common.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/GazeboYarpPlugins) -endif() diff --git a/libraries/singleton/CMakeLists.txt b/libraries/singleton/CMakeLists.txt index fd800899c..58629cebe 100644 --- a/libraries/singleton/CMakeLists.txt +++ b/libraries/singleton/CMakeLists.txt @@ -2,7 +2,7 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.11) + PROJECT(Library_Singleton) diff --git a/plugins/basestate/CMakeLists.txt b/plugins/basestate/CMakeLists.txt index 73d47e283..d6c6ed979 100644 --- a/plugins/basestate/CMakeLists.txt +++ b/plugins/basestate/CMakeLists.txt @@ -2,16 +2,10 @@ # Authors: see AUTHORS file. # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME basestate @@ -19,7 +13,7 @@ add_gazebo_yarp_plugin_target(LIBRARY_NAME basestate INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${SDFORMAT_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${SDFORMAT_LIBRARIES} HEADERS include/gazebo/BaseState.hh include/yarp/dev/BaseStateDriver.h SOURCES src/BaseState.cc diff --git a/plugins/camera/CMakeLists.txt b/plugins/camera/CMakeLists.txt index c8d1071d9..1c7f6fd24 100644 --- a/plugins/camera/CMakeLists.txt +++ b/plugins/camera/CMakeLists.txt @@ -2,22 +2,19 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) + PROJECT(Plugin_Camera) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() + + add_gazebo_yarp_plugin_target(LIBRARY_NAME camera INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} CameraPlugin + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} CameraPlugin HEADERS include/gazebo/Camera.hh include/yarp/dev/CameraDriver.h SOURCES src/Camera.cc diff --git a/plugins/clock/CMakeLists.txt b/plugins/clock/CMakeLists.txt index 8b3760367..dd5f40027 100644 --- a/plugins/clock/CMakeLists.txt +++ b/plugins/clock/CMakeLists.txt @@ -2,22 +2,17 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) PROJECT(Plugin_Clock) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() + add_gazebo_yarp_plugin_target(LIBRARY_NAME clock INCLUDE_DIRS include/gazebo include/GazeboYarpPlugins SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES gazebo_yarp_rpc_clock ${LIB_COMMON_NAME} ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_rpc_clock gazebo_yarp_lib_common ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS include/gazebo/Clock.hh include/GazeboYarpPlugins/ClockServerImpl.h SOURCES src/Clock.cc diff --git a/plugins/configurationoverride/CMakeLists.txt b/plugins/configurationoverride/CMakeLists.txt index 2fca925ea..672961a79 100644 --- a/plugins/configurationoverride/CMakeLists.txt +++ b/plugins/configurationoverride/CMakeLists.txt @@ -11,13 +11,12 @@ include(AddGazeboYarpPluginTarget) set(configurationoverride_sources src/ConfigurationOverride.cc) set(configurationoverride_headers include/gazebo/ConfigurationOverride.hh) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) add_gazebo_yarp_plugin_target(LIBRARY_NAME configurationoverride INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR} INCLUDE_DIRS include/gazebo include/GazeboYarpPlugins SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS ${configurationoverride_headers} SOURCES ${configurationoverride_sources} - ) \ No newline at end of file + ) diff --git a/plugins/contactloadcellarray/CMakeLists.txt b/plugins/contactloadcellarray/CMakeLists.txt index 64744638d..78a9880b8 100644 --- a/plugins/contactloadcellarray/CMakeLists.txt +++ b/plugins/contactloadcellarray/CMakeLists.txt @@ -2,21 +2,15 @@ # Authors: see AUTHORS file. # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME contactloadcellarray INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR} INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${SDFORMAT_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${SDFORMAT_LIBRARIES} HEADERS include/gazebo/ContactLoadCellArray.hh include/yarp/dev/ContactLoadCellArrayDriver.h SOURCES src/ContactLoadCellArray.cc diff --git a/plugins/controlboard/CMakeLists.txt b/plugins/controlboard/CMakeLists.txt index f9a1d661c..07e82b592 100644 --- a/plugins/controlboard/CMakeLists.txt +++ b/plugins/controlboard/CMakeLists.txt @@ -2,20 +2,12 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) - PROJECT(Plugin_ControlBoard) # Copied from YARP set(CMAKE_CXX_EXTENSIONS OFF) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED 11) -if(NOT CMAKE_MINIMUM_REQUIRED_VERSION VERSION_LESS 3.1) - message(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.") -endif() -if(${CMAKE_VERSION} VERSION_LESS 3.1) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CXX11_FLAGS}") -endif() include(AddGazeboYarpPluginTarget) @@ -45,15 +37,10 @@ set(controlBoard_headers include/gazebo/ControlBoard.hh include/yarp/dev/ControlBoardDriverCoupling.h) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME controlboard INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS ${controlBoard_headers} SOURCES ${controlBoard_source}) diff --git a/plugins/depthCamera/CMakeLists.txt b/plugins/depthCamera/CMakeLists.txt index f5da0f011..203fb4372 100644 --- a/plugins/depthCamera/CMakeLists.txt +++ b/plugins/depthCamera/CMakeLists.txt @@ -8,16 +8,12 @@ PROJECT(Plugin_DepthCamera) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() + add_gazebo_yarp_plugin_target(LIBRARY_NAME depthCamera INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} DepthCameraPlugin + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} DepthCameraPlugin HEADERS include/gazebo/DepthCamera.hh include/yarp/dev/DepthCameraDriver.h SOURCES src/DepthCamera.cc diff --git a/plugins/externalwrench/CMakeLists.txt b/plugins/externalwrench/CMakeLists.txt index 485463984..a563d4aa4 100644 --- a/plugins/externalwrench/CMakeLists.txt +++ b/plugins/externalwrench/CMakeLists.txt @@ -2,7 +2,6 @@ # Authors: Jorhabib Eljaik, Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro and Alessandro Settimi # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) PROJECT(Plugin_ExternalWrench) diff --git a/plugins/fakecontrolboard/CMakeLists.txt b/plugins/fakecontrolboard/CMakeLists.txt index e3c9699e7..79e67efca 100644 --- a/plugins/fakecontrolboard/CMakeLists.txt +++ b/plugins/fakecontrolboard/CMakeLists.txt @@ -2,7 +2,6 @@ # Authors: see AUTHORS file. # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) include(AddGazeboYarpPluginTarget) @@ -20,17 +19,11 @@ set(plugin_source src/FakeControlBoard.cc set(plugin_headers include/gazebo/FakeControlBoard.hh include/yarp/dev/FakeControlBoardDriver.h) - -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() - + add_gazebo_yarp_plugin_target(LIBRARY_NAME fakecontrolboard INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS ${plugin_headers} SOURCES ${plugin_source} ) diff --git a/plugins/forcetorque/CMakeLists.txt b/plugins/forcetorque/CMakeLists.txt index 1dfec21a2..3ba77a573 100644 --- a/plugins/forcetorque/CMakeLists.txt +++ b/plugins/forcetorque/CMakeLists.txt @@ -2,22 +2,17 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) + PROJECT(Plugin_ForceTorque) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME forcetorque INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ForceTorquePlugin ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ForceTorquePlugin ${Boost_LIBRARIES} HEADERS include/gazebo/ForceTorque.hh include/yarp/dev/ForceTorqueDriver.h SOURCES src/ForceTorque.cc diff --git a/plugins/imu/CMakeLists.txt b/plugins/imu/CMakeLists.txt index c72457fa2..e8ac89375 100644 --- a/plugins/imu/CMakeLists.txt +++ b/plugins/imu/CMakeLists.txt @@ -2,22 +2,16 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) PROJECT(Plugin_IMU) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME imu INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS include/gazebo/IMU.hh include/yarp/dev/IMUDriver.h SOURCES src/IMU.cc diff --git a/plugins/inertialmtb/CMakeLists.txt b/plugins/inertialmtb/CMakeLists.txt index 7ea7094c0..7d426f10b 100644 --- a/plugins/inertialmtb/CMakeLists.txt +++ b/plugins/inertialmtb/CMakeLists.txt @@ -2,23 +2,18 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi, Francesco Romano and Nuno Guedelha # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) + PROJECT(Plugin_inertial) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME inertial INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR} INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS include/gazebo/inertialMTB.hh SOURCES src/inertialMTB.cc ) diff --git a/plugins/inertialmtbPart/CMakeLists.txt b/plugins/inertialmtbPart/CMakeLists.txt index 07699756d..1a27e47e0 100644 --- a/plugins/inertialmtbPart/CMakeLists.txt +++ b/plugins/inertialmtbPart/CMakeLists.txt @@ -2,23 +2,17 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi, Francesco Romano and Nuno Guedelha # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) PROJECT(Plugin_distributedinertials) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME distributedinertials INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR} INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS include/gazebo/inertialMTBPart.hh include/yarp/dev/inertialMTBPartDriver.h SOURCES src/inertialMTBPart.cc diff --git a/plugins/lasersensor/CMakeLists.txt b/plugins/lasersensor/CMakeLists.txt index 491d54c76..1f90bf440 100644 --- a/plugins/lasersensor/CMakeLists.txt +++ b/plugins/lasersensor/CMakeLists.txt @@ -2,22 +2,16 @@ # Authors: Marco Randazzo # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) PROJECT(Plugin_LaserSensor) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME lasersensor INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} RayPlugin ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} RayPlugin ${Boost_LIBRARIES} HEADERS include/gazebo/LaserSensor.hh include/yarp/dev/LaserSensorDriver.h SOURCES src/LaserSensor.cc diff --git a/plugins/linkattacher/CMakeLists.txt b/plugins/linkattacher/CMakeLists.txt index 562574f1c..0784934b8 100644 --- a/plugins/linkattacher/CMakeLists.txt +++ b/plugins/linkattacher/CMakeLists.txt @@ -11,13 +11,12 @@ include(AddGazeboYarpPluginTarget) set(linkattacher_sources src/linkattacherserverimpl.cpp src/linkattacher.cc) set(linkattacher_headers include/GazeboYarpPlugins/linkattacherserverimpl.h include/gazebo/linkattacher.hh) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) add_gazebo_yarp_plugin_target(LIBRARY_NAME linkattacher INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR} INCLUDE_DIRS include/gazebo include/GazeboYarpPlugins SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES gazebo_yarp_rpc_linkattacher ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_rpc_linkattacher gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS ${linkattacher_headers} SOURCES ${linkattacher_sources} ) diff --git a/plugins/maissensor/CMakeLists.txt b/plugins/maissensor/CMakeLists.txt index dc3720ab1..27c917d45 100644 --- a/plugins/maissensor/CMakeLists.txt +++ b/plugins/maissensor/CMakeLists.txt @@ -2,7 +2,7 @@ # Authors: Marco Randazzo # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) + PROJECT(Plugin_MaisSensor) @@ -16,16 +16,11 @@ set(controlBoard_headers include/gazebo/MaisSensor.hh include/yarp/dev/MaisSensorDriver.h) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() - + add_gazebo_yarp_plugin_target(LIBRARY_NAME maissensor INCLUDE_DIRS include/gazebo include/yarp/dev SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS ${controlBoard_headers} SOURCES ${controlBoard_source} ) diff --git a/plugins/modelposepublisher/CMakeLists.txt b/plugins/modelposepublisher/CMakeLists.txt index cfabedcf9..2fb7c70e8 100644 --- a/plugins/modelposepublisher/CMakeLists.txt +++ b/plugins/modelposepublisher/CMakeLists.txt @@ -2,22 +2,16 @@ # Authors: see AUTHORS file. # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) PROJECT(Plugin_ModelPosePublisher) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME modelposepublisher INCLUDE_DIRS include/gazebo SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_lib_common ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS include/gazebo/ModelPosePublisher.hh SOURCES src/ModelPosePublisher.cc ) diff --git a/plugins/multicamera/CMakeLists.txt b/plugins/multicamera/CMakeLists.txt index 4cb6ba6a2..7d2b05237 100644 --- a/plugins/multicamera/CMakeLists.txt +++ b/plugins/multicamera/CMakeLists.txt @@ -2,17 +2,11 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) project(Plugin_MultiCamera) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() include_directories(/usr/local/src/robot/gazebo-yarp-plugins/libraries/singleton/include) @@ -34,7 +28,7 @@ add_gazebo_yarp_plugin_target(LIBRARY_NAME multicamera ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton gazebo_yarp_MultiCameraPlugin ${YARP_LIBRARIES} diff --git a/plugins/showmodelcom/CMakeLists.txt b/plugins/showmodelcom/CMakeLists.txt index cc3060c0b..b278178dd 100644 --- a/plugins/showmodelcom/CMakeLists.txt +++ b/plugins/showmodelcom/CMakeLists.txt @@ -2,7 +2,6 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) PROJECT(Plugin_ShowModelCoM) diff --git a/plugins/videotexture/CMakeLists.txt b/plugins/videotexture/CMakeLists.txt index 5a5ef8c73..9345e7265 100644 --- a/plugins/videotexture/CMakeLists.txt +++ b/plugins/videotexture/CMakeLists.txt @@ -2,17 +2,12 @@ # Authors: Marco Randazzo # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) PROJECT(Plugin_Videotexture) include(AddGazeboYarpPluginTarget) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() + # find opencv dependency find_package(OpenCV REQUIRED) @@ -27,7 +22,7 @@ add_gazebo_yarp_plugin_target(LIBRARY_NAME videotexture ${PROTOBUF_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} - LINKED_LIBRARIES ${LIB_COMMON_NAME} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} diff --git a/plugins/worldinterface/CMakeLists.txt b/plugins/worldinterface/CMakeLists.txt index 91ae8ebac..a7a8c3f2f 100644 --- a/plugins/worldinterface/CMakeLists.txt +++ b/plugins/worldinterface/CMakeLists.txt @@ -2,7 +2,7 @@ # Authors: Lorenzo Natale # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.7) + PROJECT(Plugin_WorldInterface) @@ -10,16 +10,11 @@ include(AddGazeboYarpPluginTarget) set(worldinterface_source src/worldinterface.cpp src/worldinterfaceserverimpl.cpp src/worldproxy.cpp) -set(LIB_COMMON_NAME gazebo_yarp_lib_common) -if(CMAKE_VERSION VERSION_LESS 3.0.0) - get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS) - unset(LIB_COMMON_NAME) -endif() add_gazebo_yarp_plugin_target(LIBRARY_NAME worldinterface INCLUDE_DIRS include/gazebo SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} - LINKED_LIBRARIES gazebo_yarp_rpc_worldinterface ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + LINKED_LIBRARIES gazebo_yarp_rpc_worldinterface gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} HEADERS ${worldinterface_headers} src/worldinterfaceserverimpl.h src/worldproxy.h diff --git a/thrift/clock/CMakeLists.txt b/thrift/clock/CMakeLists.txt index 125829704..1227cc6e2 100644 --- a/thrift/clock/CMakeLists.txt +++ b/thrift/clock/CMakeLists.txt @@ -2,7 +2,6 @@ # Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.11) set(LIBRARY_TARGET_NAME gazebo_yarp_rpc_clock) diff --git a/thrift/worldinterface/CMakeLists.txt b/thrift/worldinterface/CMakeLists.txt index 629ba5b28..2d1b07937 100644 --- a/thrift/worldinterface/CMakeLists.txt +++ b/thrift/worldinterface/CMakeLists.txt @@ -2,7 +2,6 @@ # Authors: Lorenzo Natale # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT -cmake_minimum_required(VERSION 2.8.11) set(LIBRARY_TARGET_NAME gazebo_yarp_rpc_worldinterface) From f52c36e8e45de21d2246161464d3a1f905c505e0 Mon Sep 17 00:00:00 2001 From: Luca Tricerri Date: Mon, 6 May 2019 22:43:43 +0200 Subject: [PATCH 21/25] Add resetSimulationState and regenerate thrift-generated files (#424) Co-Authored-By: triccyx --- .../GazeboYarpPlugins/ClockServerImpl.h | 1 + plugins/clock/include/gazebo/Clock.hh | 5 + plugins/clock/src/Clock.cc | 8 +- plugins/clock/src/ClockServerImpl.cpp | 5 + .../autogenerated/clock_rpc_thrift.cmake | 2 +- .../clock/autogenerated/include/ClockServer.h | 125 +- .../clock/autogenerated/src/ClockServer.cpp | 1069 +++--- thrift/clock/clock_rpc.thrift | 5 + .../include/LinkAttacherServer.h | 79 +- .../linkattacher_rpc_thrift.cmake | 2 +- .../autogenerated/src/LinkAttacherServer.cpp | 629 ++-- .../autogenerated/include/Color.h | 350 +- .../autogenerated/include/Pose.h | 483 ++- .../include/WorldInterfaceServer.h | 302 +- .../autogenerated/src/Color.cpp | 811 +++-- .../worldinterface/autogenerated/src/Pose.cpp | 1276 +++++-- .../src/WorldInterfaceServer.cpp | 2968 ++++++++++------- .../worldinterface_rpc_thrift.cmake | 2 +- 18 files changed, 4912 insertions(+), 3210 deletions(-) diff --git a/plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h b/plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h index c35eb2813..c02ad14ca 100644 --- a/plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h +++ b/plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h @@ -27,6 +27,7 @@ class GazeboYarpPlugins::ClockServerImpl : public GazeboYarpPlugins::ClockServer virtual void resetSimulation(); virtual double getSimulationTime(); virtual double getStepSize(); + virtual void resetSimulationState(); private: diff --git a/plugins/clock/include/gazebo/Clock.hh b/plugins/clock/include/gazebo/Clock.hh index e60b96052..565d68b5b 100644 --- a/plugins/clock/include/gazebo/Clock.hh +++ b/plugins/clock/include/gazebo/Clock.hh @@ -101,6 +101,11 @@ public: */ void resetSimulationTime(); + /** + * Reset the simulation state back to initial state + */ + void resetSimulationState(); + /** * Reset the simulation time and state back to zero */ diff --git a/plugins/clock/src/Clock.cc b/plugins/clock/src/Clock.cc index 62cff254f..1fedbea36 100644 --- a/plugins/clock/src/Clock.cc +++ b/plugins/clock/src/Clock.cc @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -30,7 +31,6 @@ namespace gazebo , m_rpcPort(0) , m_clockServer(0) { - } GazeboYarpClock::~GazeboYarpClock() @@ -182,6 +182,12 @@ namespace gazebo m_world->Reset(); } + void GazeboYarpClock::resetSimulationState() + { + m_world->ResetEntities(gazebo::physics::Base::BASE); + event::Events::worldReset(); + } + common::Time GazeboYarpClock::getSimulationTime() { #if GAZEBO_MAJOR_VERSION >= 8 diff --git a/plugins/clock/src/ClockServerImpl.cpp b/plugins/clock/src/ClockServerImpl.cpp index 2b0373f1e..a52cceef5 100644 --- a/plugins/clock/src/ClockServerImpl.cpp +++ b/plugins/clock/src/ClockServerImpl.cpp @@ -52,4 +52,9 @@ namespace GazeboYarpPlugins { { return m_clock.getStepSize(); } + + void ClockServerImpl::resetSimulationState() + { + m_clock.resetSimulationState(); + } } diff --git a/thrift/clock/autogenerated/clock_rpc_thrift.cmake b/thrift/clock/autogenerated/clock_rpc_thrift.cmake index f39ac91f9..d161846b5 100644 --- a/thrift/clock/autogenerated/clock_rpc_thrift.cmake +++ b/thrift/clock/autogenerated/clock_rpc_thrift.cmake @@ -1,4 +1,4 @@ -# Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) +# Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) # All rights reserved. # # This software may be modified and distributed under the terms of the diff --git a/thrift/clock/autogenerated/include/ClockServer.h b/thrift/clock/autogenerated/include/ClockServer.h index 2aab41ecf..61f5c8beb 100644 --- a/thrift/clock/autogenerated/include/ClockServer.h +++ b/thrift/clock/autogenerated/include/ClockServer.h @@ -1,71 +1,92 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. -#ifndef YARP_THRIFT_GENERATOR_ClockServer -#define YARP_THRIFT_GENERATOR_ClockServer +#ifndef YARP_THRIFT_GENERATOR_SERVICE_CLOCKSERVER_H +#define YARP_THRIFT_GENERATOR_SERVICE_CLOCKSERVER_H #include #include namespace GazeboYarpPlugins { - class ClockServer; -} - -class GazeboYarpPlugins::ClockServer : public yarp::os::Wire { +class ClockServer : + public yarp::os::Wire +{ public: - ClockServer(); - /** - * Pause the simulation if it was running - */ - virtual void pauseSimulation(); - /** - * Resume the simulation if it was paused - */ - virtual void continueSimulation(); - /** - * Steps the simulation for the provided number of steps. - * The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed) - * @note: this function (will be) not blocking, i.e. it will return immediately. Currently calling this function - * twice before the previous call actually ends its computation gives and undefined behavior. - * @param numberOfSteps number of steps to simulate - */ - virtual void stepSimulation(const std::int32_t numberOfSteps = 1); - /** - * Steps the simulation for the provided number of steps. - * The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed) - * @note: this function is blocking - * @param numberOfSteps number of steps to simulate - */ - virtual void stepSimulationAndWait(const std::int32_t numberOfSteps = 1); - /** - * Reset the simulation time back to zero - */ - virtual void resetSimulationTime(); - /** - * Get the current simulation time - * @return the simulation time. - */ - virtual double getSimulationTime(); - /** - * Get the current step size in seconds. - * @return the step size in seconds - */ - virtual double getStepSize(); - /** - * Reset the simulation state and time - */ - virtual void resetSimulation(); - virtual bool read(yarp::os::ConnectionReader& connection) override; - virtual std::vector help(const std::string& functionName="--all"); + // Constructor + ClockServer(); + + /** + * Pause the simulation if it was running + */ + virtual void pauseSimulation(); + + /** + * Resume the simulation if it was paused + */ + virtual void continueSimulation(); + + /** + * Steps the simulation for the provided number of steps. + * The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed) + * @note: this function (will be) not blocking, i.e. it will return immediately. Currently calling this function + * twice before the previous call actually ends its computation gives and undefined behavior. + * @param numberOfSteps number of steps to simulate + */ + virtual void stepSimulation(const std::int32_t numberOfSteps = 1); + + /** + * Steps the simulation for the provided number of steps. + * The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed) + * @note: this function is blocking + * @param numberOfSteps number of steps to simulate + */ + virtual void stepSimulationAndWait(const std::int32_t numberOfSteps = 1); + + /** + * Reset the simulation time back to zero + */ + virtual void resetSimulationTime(); + + /** + * Get the current simulation time + * @return the simulation time. + */ + virtual double getSimulationTime(); + + /** + * Get the current step size in seconds. + * @return the step size in seconds + */ + virtual double getStepSize(); + + /** + * Reset the simulation state and time + */ + virtual void resetSimulation(); + + /** + * Reset the simulation state back to initial pose + */ + virtual void resetSimulationState(); + + // help method + virtual std::vector help(const std::string& functionName = "--all"); + + // read from ConnectionReader + bool read(yarp::os::ConnectionReader& connection) override; }; -#endif +} // namespace GazeboYarpPlugins + +#endif // YARP_THRIFT_GENERATOR_SERVICE_CLOCKSERVER_H diff --git a/thrift/clock/autogenerated/src/ClockServer.cpp b/thrift/clock/autogenerated/src/ClockServer.cpp index cdbad17e8..442b8fd55 100644 --- a/thrift/clock/autogenerated/src/ClockServer.cpp +++ b/thrift/clock/autogenerated/src/ClockServer.cpp @@ -1,508 +1,715 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. #include + #include namespace GazeboYarpPlugins { - -class ClockServer_pauseSimulation : public yarp::os::Portable { +class ClockServer_pauseSimulation : + public yarp::os::Portable +{ public: - void init(); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + void init(); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class ClockServer_continueSimulation : public yarp::os::Portable { -public: - void init(); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +bool ClockServer_pauseSimulation::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeTag("pauseSimulation", 1, 1)) { + return false; + } + return true; +} -class ClockServer_stepSimulation : public yarp::os::Portable { +bool ClockServer_pauseSimulation::read(yarp::os::ConnectionReader& connection) +{ + YARP_UNUSED(connection); + return true; +} + +void ClockServer_pauseSimulation::init() +{ +} + +class ClockServer_continueSimulation : + public yarp::os::Portable +{ public: - std::int32_t numberOfSteps; - void init(const std::int32_t numberOfSteps); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + void init(); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class ClockServer_stepSimulationAndWait : public yarp::os::Portable { +bool ClockServer_continueSimulation::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeTag("continueSimulation", 1, 1)) { + return false; + } + return true; +} + +bool ClockServer_continueSimulation::read(yarp::os::ConnectionReader& connection) +{ + YARP_UNUSED(connection); + return true; +} + +void ClockServer_continueSimulation::init() +{ +} + +class ClockServer_stepSimulation : + public yarp::os::Portable +{ public: - std::int32_t numberOfSteps; - void init(const std::int32_t numberOfSteps); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + std::int32_t numberOfSteps; + void init(const std::int32_t numberOfSteps); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class ClockServer_resetSimulationTime : public yarp::os::Portable { +bool ClockServer_stepSimulation::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(2)) { + return false; + } + if (!writer.writeTag("stepSimulation", 1, 1)) { + return false; + } + if (!writer.writeI32(numberOfSteps)) { + return false; + } + return true; +} + +bool ClockServer_stepSimulation::read(yarp::os::ConnectionReader& connection) +{ + YARP_UNUSED(connection); + return true; +} + +void ClockServer_stepSimulation::init(const std::int32_t numberOfSteps) +{ + this->numberOfSteps = numberOfSteps; +} + +class ClockServer_stepSimulationAndWait : + public yarp::os::Portable +{ public: - void init(); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + std::int32_t numberOfSteps; + void init(const std::int32_t numberOfSteps); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class ClockServer_getSimulationTime : public yarp::os::Portable { +bool ClockServer_stepSimulationAndWait::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(2)) { + return false; + } + if (!writer.writeTag("stepSimulationAndWait", 1, 1)) { + return false; + } + if (!writer.writeI32(numberOfSteps)) { + return false; + } + return true; +} + +bool ClockServer_stepSimulationAndWait::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + return true; +} + +void ClockServer_stepSimulationAndWait::init(const std::int32_t numberOfSteps) +{ + this->numberOfSteps = numberOfSteps; +} + +class ClockServer_resetSimulationTime : + public yarp::os::Portable +{ public: - double _return; - void init(); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + void init(); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class ClockServer_getStepSize : public yarp::os::Portable { +bool ClockServer_resetSimulationTime::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeTag("resetSimulationTime", 1, 1)) { + return false; + } + return true; +} + +bool ClockServer_resetSimulationTime::read(yarp::os::ConnectionReader& connection) +{ + YARP_UNUSED(connection); + return true; +} + +void ClockServer_resetSimulationTime::init() +{ +} + +class ClockServer_getSimulationTime : + public yarp::os::Portable +{ public: - double _return; - void init(); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + double _return; + void init(); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class ClockServer_resetSimulation : public yarp::os::Portable { +bool ClockServer_getSimulationTime::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeTag("getSimulationTime", 1, 1)) { + return false; + } + return true; +} + +bool ClockServer_getSimulationTime::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readFloat64(_return)) { + reader.fail(); + return false; + } + return true; +} + +void ClockServer_getSimulationTime::init() +{ + _return = (double)0; +} + +class ClockServer_getStepSize : + public yarp::os::Portable +{ public: - void init(); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + double _return; + void init(); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -bool ClockServer_pauseSimulation::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(1)) return false; - if (!writer.writeTag("pauseSimulation",1,1)) return false; - return true; +bool ClockServer_getStepSize::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeTag("getStepSize", 1, 1)) { + return false; + } + return true; } -bool ClockServer_pauseSimulation::read(yarp::os::ConnectionReader& connection) { - YARP_UNUSED(connection); - return true; +bool ClockServer_getStepSize::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readFloat64(_return)) { + reader.fail(); + return false; + } + return true; } -void ClockServer_pauseSimulation::init() { +void ClockServer_getStepSize::init() +{ + _return = (double)0; } -bool ClockServer_continueSimulation::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(1)) return false; - if (!writer.writeTag("continueSimulation",1,1)) return false; - return true; -} +class ClockServer_resetSimulation : + public yarp::os::Portable +{ +public: + void init(); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; -bool ClockServer_continueSimulation::read(yarp::os::ConnectionReader& connection) { - YARP_UNUSED(connection); - return true; +bool ClockServer_resetSimulation::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeTag("resetSimulation", 1, 1)) { + return false; + } + return true; } -void ClockServer_continueSimulation::init() { +bool ClockServer_resetSimulation::read(yarp::os::ConnectionReader& connection) +{ + YARP_UNUSED(connection); + return true; } -bool ClockServer_stepSimulation::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("stepSimulation",1,1)) return false; - if (!writer.writeI32(numberOfSteps)) return false; - return true; +void ClockServer_resetSimulation::init() +{ } -bool ClockServer_stepSimulation::read(yarp::os::ConnectionReader& connection) { - YARP_UNUSED(connection); - return true; +class ClockServer_resetSimulationState : + public yarp::os::Portable +{ +public: + void init(); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; + +bool ClockServer_resetSimulationState::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeTag("resetSimulationState", 1, 1)) { + return false; + } + return true; } -void ClockServer_stepSimulation::init(const std::int32_t numberOfSteps) { - this->numberOfSteps = numberOfSteps; +bool ClockServer_resetSimulationState::read(yarp::os::ConnectionReader& connection) +{ + YARP_UNUSED(connection); + return true; } -bool ClockServer_stepSimulationAndWait::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("stepSimulationAndWait",1,1)) return false; - if (!writer.writeI32(numberOfSteps)) return false; - return true; +void ClockServer_resetSimulationState::init() +{ } -bool ClockServer_stepSimulationAndWait::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - return true; +// Constructor +ClockServer::ClockServer() +{ + yarp().setOwner(*this); } -void ClockServer_stepSimulationAndWait::init(const std::int32_t numberOfSteps) { - this->numberOfSteps = numberOfSteps; +void ClockServer::pauseSimulation() +{ + ClockServer_pauseSimulation helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "void ClockServer::pauseSimulation()"); + } + yarp().write(helper); } -bool ClockServer_resetSimulationTime::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(1)) return false; - if (!writer.writeTag("resetSimulationTime",1,1)) return false; - return true; +void ClockServer::continueSimulation() +{ + ClockServer_continueSimulation helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "void ClockServer::continueSimulation()"); + } + yarp().write(helper); } -bool ClockServer_resetSimulationTime::read(yarp::os::ConnectionReader& connection) { - YARP_UNUSED(connection); - return true; +void ClockServer::stepSimulation(const std::int32_t numberOfSteps) +{ + ClockServer_stepSimulation helper; + helper.init(numberOfSteps); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "void ClockServer::stepSimulation(const std::int32_t numberOfSteps)"); + } + yarp().write(helper); } -void ClockServer_resetSimulationTime::init() { +void ClockServer::stepSimulationAndWait(const std::int32_t numberOfSteps) +{ + ClockServer_stepSimulationAndWait helper; + helper.init(numberOfSteps); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "void ClockServer::stepSimulationAndWait(const std::int32_t numberOfSteps)"); + } + yarp().write(helper, helper); } -bool ClockServer_getSimulationTime::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(1)) return false; - if (!writer.writeTag("getSimulationTime",1,1)) return false; - return true; +void ClockServer::resetSimulationTime() +{ + ClockServer_resetSimulationTime helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "void ClockServer::resetSimulationTime()"); + } + yarp().write(helper); } -bool ClockServer_getSimulationTime::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readFloat64(_return)) { - reader.fail(); - return false; - } - return true; +double ClockServer::getSimulationTime() +{ + double _return = (double)0; + ClockServer_getSimulationTime helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "double ClockServer::getSimulationTime()"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; } -void ClockServer_getSimulationTime::init() { - _return = (double)0; +double ClockServer::getStepSize() +{ + double _return = (double)0; + ClockServer_getStepSize helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "double ClockServer::getStepSize()"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; } -bool ClockServer_getStepSize::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(1)) return false; - if (!writer.writeTag("getStepSize",1,1)) return false; - return true; +void ClockServer::resetSimulation() +{ + ClockServer_resetSimulation helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "void ClockServer::resetSimulation()"); + } + yarp().write(helper); } -bool ClockServer_getStepSize::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readFloat64(_return)) { - reader.fail(); - return false; - } - return true; -} - -void ClockServer_getStepSize::init() { - _return = (double)0; -} - -bool ClockServer_resetSimulation::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(1)) return false; - if (!writer.writeTag("resetSimulation",1,1)) return false; - return true; -} - -bool ClockServer_resetSimulation::read(yarp::os::ConnectionReader& connection) { - YARP_UNUSED(connection); - return true; -} - -void ClockServer_resetSimulation::init() { -} - -ClockServer::ClockServer() { - yarp().setOwner(*this); -} -void ClockServer::pauseSimulation() { - ClockServer_pauseSimulation helper; - helper.init(); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","void ClockServer::pauseSimulation()"); - } - yarp().write(helper); -} -void ClockServer::continueSimulation() { - ClockServer_continueSimulation helper; - helper.init(); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","void ClockServer::continueSimulation()"); - } - yarp().write(helper); -} -void ClockServer::stepSimulation(const std::int32_t numberOfSteps) { - ClockServer_stepSimulation helper; - helper.init(numberOfSteps); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","void ClockServer::stepSimulation(const std::int32_t numberOfSteps)"); - } - yarp().write(helper); -} -void ClockServer::stepSimulationAndWait(const std::int32_t numberOfSteps) { - ClockServer_stepSimulationAndWait helper; - helper.init(numberOfSteps); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","void ClockServer::stepSimulationAndWait(const std::int32_t numberOfSteps)"); - } - yarp().write(helper,helper); -} -void ClockServer::resetSimulationTime() { - ClockServer_resetSimulationTime helper; - helper.init(); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","void ClockServer::resetSimulationTime()"); - } - yarp().write(helper); -} -double ClockServer::getSimulationTime() { - double _return = (double)0; - ClockServer_getSimulationTime helper; - helper.init(); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","double ClockServer::getSimulationTime()"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -double ClockServer::getStepSize() { - double _return = (double)0; - ClockServer_getStepSize helper; - helper.init(); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","double ClockServer::getStepSize()"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -void ClockServer::resetSimulation() { - ClockServer_resetSimulation helper; - helper.init(); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","void ClockServer::resetSimulation()"); - } - yarp().write(helper); -} - -bool ClockServer::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - reader.expectAccept(); - if (!reader.readListHeader()) { reader.fail(); return false; } - std::string tag = reader.readTag(); - bool direct = (tag=="__direct__"); - if (direct) tag = reader.readTag(); - while (!reader.isError()) { - // TODO: use quick lookup, this is just a test - if (tag == "pauseSimulation") { - if (!direct) { - ClockServer_pauseSimulation helper; - helper.init(); - yarp().callback(helper,*this,"__direct__"); - } else { - pauseSimulation(); - } - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeOnewayResponse()) return false; - } - reader.accept(); - return true; - } - if (tag == "continueSimulation") { - if (!direct) { - ClockServer_continueSimulation helper; - helper.init(); - yarp().callback(helper,*this,"__direct__"); - } else { - continueSimulation(); - } - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeOnewayResponse()) return false; - } - reader.accept(); - return true; - } - if (tag == "stepSimulation") { - std::int32_t numberOfSteps; - if (!reader.readI32(numberOfSteps)) { - numberOfSteps = 1; - } - if (!direct) { - ClockServer_stepSimulation helper; - helper.init(numberOfSteps); - yarp().callback(helper,*this,"__direct__"); - } else { - stepSimulation(numberOfSteps); - } - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeOnewayResponse()) return false; - } - reader.accept(); - return true; - } - if (tag == "stepSimulationAndWait") { - std::int32_t numberOfSteps; - if (!reader.readI32(numberOfSteps)) { - numberOfSteps = 1; - } - stepSimulationAndWait(numberOfSteps); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(0)) return false; - } - reader.accept(); - return true; - } - if (tag == "resetSimulationTime") { - if (!direct) { - ClockServer_resetSimulationTime helper; - helper.init(); - yarp().callback(helper,*this,"__direct__"); - } else { - resetSimulationTime(); - } - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeOnewayResponse()) return false; - } - reader.accept(); - return true; - } - if (tag == "getSimulationTime") { - double _return; - _return = getSimulationTime(); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeFloat64(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "getStepSize") { - double _return; - _return = getStepSize(); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeFloat64(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "resetSimulation") { - if (!direct) { - ClockServer_resetSimulation helper; - helper.init(); - yarp().callback(helper,*this,"__direct__"); - } else { - resetSimulation(); - } - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeOnewayResponse()) return false; - } - reader.accept(); - return true; - } - if (tag == "help") { - std::string functionName; - if (!reader.readString(functionName)) { - functionName = "--all"; - } - std::vector _return=help(functionName); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("many",1, 0)) return false; - if (!writer.writeListBegin(BOTTLE_TAG_INT32, static_cast(_return.size()))) return false; - std::vector ::iterator _iterHelp; - for (_iterHelp = _return.begin(); _iterHelp != _return.end(); ++_iterHelp) - { - if (!writer.writeString(*_iterHelp)) return false; - } - if (!writer.writeListEnd()) return false; +void ClockServer::resetSimulationState() +{ + ClockServer_resetSimulationState helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "void ClockServer::resetSimulationState()"); + } + yarp().write(helper); +} + +// help method +std::vector ClockServer::help(const std::string& functionName) +{ + bool showAll = (functionName == "--all"); + std::vector helpString; + if (showAll) { + helpString.emplace_back("*** Available commands:"); + helpString.emplace_back("pauseSimulation"); + helpString.emplace_back("continueSimulation"); + helpString.emplace_back("stepSimulation"); + helpString.emplace_back("stepSimulationAndWait"); + helpString.emplace_back("resetSimulationTime"); + helpString.emplace_back("getSimulationTime"); + helpString.emplace_back("getStepSize"); + helpString.emplace_back("resetSimulation"); + helpString.emplace_back("resetSimulationState"); + helpString.emplace_back("help"); + } else { + if (functionName == "pauseSimulation") { + helpString.emplace_back("void pauseSimulation() "); + helpString.emplace_back("Pause the simulation if it was running "); + } + if (functionName == "continueSimulation") { + helpString.emplace_back("void continueSimulation() "); + helpString.emplace_back("Resume the simulation if it was paused "); + } + if (functionName == "stepSimulation") { + helpString.emplace_back("void stepSimulation(const std::int32_t numberOfSteps = 1) "); + helpString.emplace_back("Steps the simulation for the provided number of steps. "); + helpString.emplace_back("The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed) "); + helpString.emplace_back("@note: this function (will be) not blocking, i.e. it will return immediately. Currently calling this function "); + helpString.emplace_back("twice before the previous call actually ends its computation gives and undefined behavior. "); + helpString.emplace_back("@param numberOfSteps number of steps to simulate "); + } + if (functionName == "stepSimulationAndWait") { + helpString.emplace_back("void stepSimulationAndWait(const std::int32_t numberOfSteps = 1) "); + helpString.emplace_back("Steps the simulation for the provided number of steps. "); + helpString.emplace_back("The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed) "); + helpString.emplace_back("@note: this function is blocking "); + helpString.emplace_back("@param numberOfSteps number of steps to simulate "); } - reader.accept(); - return true; - } - if (reader.noMore()) { reader.fail(); return false; } - std::string next_tag = reader.readTag(); - if (next_tag=="") break; - tag = tag + "_" + next_tag; - } - return false; -} - -std::vector ClockServer::help(const std::string& functionName) { - bool showAll=(functionName=="--all"); - std::vector helpString; - if(showAll) { - helpString.push_back("*** Available commands:"); - helpString.push_back("pauseSimulation"); - helpString.push_back("continueSimulation"); - helpString.push_back("stepSimulation"); - helpString.push_back("stepSimulationAndWait"); - helpString.push_back("resetSimulationTime"); - helpString.push_back("getSimulationTime"); - helpString.push_back("getStepSize"); - helpString.push_back("resetSimulation"); - helpString.push_back("help"); - } - else { - if (functionName=="pauseSimulation") { - helpString.push_back("void pauseSimulation() "); - helpString.push_back("Pause the simulation if it was running "); - } - if (functionName=="continueSimulation") { - helpString.push_back("void continueSimulation() "); - helpString.push_back("Resume the simulation if it was paused "); - } - if (functionName=="stepSimulation") { - helpString.push_back("void stepSimulation(const std::int32_t numberOfSteps = 1) "); - helpString.push_back("Steps the simulation for the provided number of steps. "); - helpString.push_back("The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed) "); - helpString.push_back("@note: this function (will be) not blocking, i.e. it will return immediately. Currently calling this function "); - helpString.push_back("twice before the previous call actually ends its computation gives and undefined behavior. "); - helpString.push_back("@param numberOfSteps number of steps to simulate "); - } - if (functionName=="stepSimulationAndWait") { - helpString.push_back("void stepSimulationAndWait(const std::int32_t numberOfSteps = 1) "); - helpString.push_back("Steps the simulation for the provided number of steps. "); - helpString.push_back("The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed) "); - helpString.push_back("@note: this function is blocking "); - helpString.push_back("@param numberOfSteps number of steps to simulate "); - } - if (functionName=="resetSimulationTime") { - helpString.push_back("void resetSimulationTime() "); - helpString.push_back("Reset the simulation time back to zero "); - } - if (functionName=="getSimulationTime") { - helpString.push_back("double getSimulationTime() "); - helpString.push_back("Get the current simulation time "); - helpString.push_back("@return the simulation time. "); - } - if (functionName=="getStepSize") { - helpString.push_back("double getStepSize() "); - helpString.push_back("Get the current step size in seconds. "); - helpString.push_back("@return the step size in seconds "); - } - if (functionName=="resetSimulation") { - helpString.push_back("void resetSimulation() "); - helpString.push_back("Reset the simulation state and time "); - } - if (functionName=="help") { - helpString.push_back("std::vector help(const std::string& functionName=\"--all\")"); - helpString.push_back("Return list of available commands, or help message for a specific function"); - helpString.push_back("@param functionName name of command for which to get a detailed description. If none or '--all' is provided, print list of available commands"); - helpString.push_back("@return list of strings (one string per line)"); - } - } - if ( helpString.empty()) helpString.push_back("Command not found"); - return helpString; -} -} // namespace + if (functionName == "resetSimulationTime") { + helpString.emplace_back("void resetSimulationTime() "); + helpString.emplace_back("Reset the simulation time back to zero "); + } + if (functionName == "getSimulationTime") { + helpString.emplace_back("double getSimulationTime() "); + helpString.emplace_back("Get the current simulation time "); + helpString.emplace_back("@return the simulation time. "); + } + if (functionName == "getStepSize") { + helpString.emplace_back("double getStepSize() "); + helpString.emplace_back("Get the current step size in seconds. "); + helpString.emplace_back("@return the step size in seconds "); + } + if (functionName == "resetSimulation") { + helpString.emplace_back("void resetSimulation() "); + helpString.emplace_back("Reset the simulation state and time "); + } + if (functionName == "resetSimulationState") { + helpString.emplace_back("void resetSimulationState() "); + helpString.emplace_back("Reset the simulation state back to initial pose "); + } + if (functionName == "help") { + helpString.emplace_back("std::vector help(const std::string& functionName = \"--all\")"); + helpString.emplace_back("Return list of available commands, or help message for a specific function"); + helpString.emplace_back("@param functionName name of command for which to get a detailed description. If none or '--all' is provided, print list of available commands"); + helpString.emplace_back("@return list of strings (one string per line)"); + } + } + if (helpString.empty()) { + helpString.emplace_back("Command not found"); + } + return helpString; +} + +// read from ConnectionReader +bool ClockServer::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + reader.expectAccept(); + if (!reader.readListHeader()) { + reader.fail(); + return false; + } + std::string tag = reader.readTag(); + bool direct = (tag == "__direct__"); + if (direct) { + tag = reader.readTag(); + } + while (!reader.isError()) { + if (tag == "pauseSimulation") { + if (!direct) { + ClockServer_pauseSimulation helper; + helper.init(); + yarp().callback(helper,*this, "__direct__"); + } else { + pauseSimulation(); + } + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeOnewayResponse()) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "continueSimulation") { + if (!direct) { + ClockServer_continueSimulation helper; + helper.init(); + yarp().callback(helper,*this, "__direct__"); + } else { + continueSimulation(); + } + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeOnewayResponse()) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "stepSimulation") { + std::int32_t numberOfSteps; + if (!reader.readI32(numberOfSteps)) { + numberOfSteps = 1; + } + if (!direct) { + ClockServer_stepSimulation helper; + helper.init(numberOfSteps); + yarp().callback(helper,*this, "__direct__"); + } else { + stepSimulation(numberOfSteps); + } + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeOnewayResponse()) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "stepSimulationAndWait") { + std::int32_t numberOfSteps; + if (!reader.readI32(numberOfSteps)) { + numberOfSteps = 1; + } + stepSimulationAndWait(numberOfSteps); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(0)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "resetSimulationTime") { + if (!direct) { + ClockServer_resetSimulationTime helper; + helper.init(); + yarp().callback(helper,*this, "__direct__"); + } else { + resetSimulationTime(); + } + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeOnewayResponse()) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "getSimulationTime") { + double _return; + _return = getSimulationTime(); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeFloat64(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "getStepSize") { + double _return; + _return = getStepSize(); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeFloat64(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "resetSimulation") { + if (!direct) { + ClockServer_resetSimulation helper; + helper.init(); + yarp().callback(helper,*this, "__direct__"); + } else { + resetSimulation(); + } + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeOnewayResponse()) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "resetSimulationState") { + if (!direct) { + ClockServer_resetSimulationState helper; + helper.init(); + yarp().callback(helper,*this, "__direct__"); + } else { + resetSimulationState(); + } + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeOnewayResponse()) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "help") { + std::string functionName; + if (!reader.readString(functionName)) { + functionName = "--all"; + } + auto _return = help(functionName); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(2)) { + return false; + } + if (!writer.writeTag("many", 1, 0)) { + return false; + } + if (!writer.writeListBegin(BOTTLE_TAG_INT32, static_cast(_return.size()))) { + return false; + } + for (const auto& _ret : _return) { + if (!writer.writeString(_ret)) { + return false; + } + } + if (!writer.writeListEnd()) { + return false; + } + } + reader.accept(); + return true; + } + if (reader.noMore()) { + reader.fail(); + return false; + } + std::string next_tag = reader.readTag(); + if (next_tag == "") { + break; + } + tag.append("_").append(next_tag); + } + return false; +} +} // namespace GazeboYarpPlugins diff --git a/thrift/clock/clock_rpc.thrift b/thrift/clock/clock_rpc.thrift index 77ab2924f..adcb264d5 100644 --- a/thrift/clock/clock_rpc.thrift +++ b/thrift/clock/clock_rpc.thrift @@ -46,5 +46,10 @@ service ClockServer { * */ oneway void resetSimulation(); + + /** Reset the simulation state back to initial pose + * + */ + oneway void resetSimulationState(); } diff --git a/thrift/linkattacher/autogenerated/include/LinkAttacherServer.h b/thrift/linkattacher/autogenerated/include/LinkAttacherServer.h index c92f91086..d8ac98227 100644 --- a/thrift/linkattacher/autogenerated/include/LinkAttacherServer.h +++ b/thrift/linkattacher/autogenerated/include/LinkAttacherServer.h @@ -1,53 +1,64 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. -#ifndef YARP_THRIFT_GENERATOR_LinkAttacherServer -#define YARP_THRIFT_GENERATOR_LinkAttacherServer +#ifndef YARP_THRIFT_GENERATOR_SERVICE_LINKATTACHERSERVER_H +#define YARP_THRIFT_GENERATOR_SERVICE_LINKATTACHERSERVER_H #include #include namespace GazeboYarpPlugins { - class LinkAttacherServer; -} - -class GazeboYarpPlugins::LinkAttacherServer : public yarp::os::Wire { +class LinkAttacherServer : + public yarp::os::Wire +{ public: - LinkAttacherServer(); - /** - * Enable/disables gravity for a model - * @param model_name name that identifies model in gazebo (that are already spawned in gazebo) - * @param enable 1 to enable gravity, 0 otherwise - * @return returns true or false on success failure - */ - virtual bool enableGravity(const std::string& model_name, const bool enable); - /** - * Attach any link of the models spawned in gazebo to a link of the robot. - * @param model_name name that identifies model in gazebo (that are already spawned in gazebo) - * @param model_link_name name of a the link in the model you want to attach to the robot - * @param robot_name name of the robot - * @param robot_link_name name of the robot link to which you want to attached the model link - * @return true if success, false otherwise - */ - virtual bool attachUnscoped(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name); - /** - * Detach a previously attached model link. - * @param model_name name that identifies model in gazebo (that are already spawned in gazebo) - * @param model_link_name name of a the link in the model that is attached to the robot - * @return true if success, false otherwise - */ - virtual bool detachUnscoped(const std::string& model_name, const std::string& model_link_name); - virtual bool read(yarp::os::ConnectionReader& connection) override; - virtual std::vector help(const std::string& functionName="--all"); + // Constructor + LinkAttacherServer(); + + /** + * Enable/disables gravity for a model + * @param model_name name that identifies model in gazebo (that are already spawned in gazebo) + * @param enable 1 to enable gravity, 0 otherwise + * @return returns true or false on success failure + */ + virtual bool enableGravity(const std::string& model_name, const bool enable); + + /** + * Attach any link of the models spawned in gazebo to a link of the robot using a fixed joint. + * @param model_name name that identifies model in gazebo (that are already spawned in gazebo) + * @param model_link_name name of a the link in the model you want to attach to the robot + * @param robot_name name of the robot + * @param robot_link_name name of the robot link to which you want to attached the model link + * @return true if success, false otherwise + */ + virtual bool attachUnscoped(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name); + + /** + * Detach the model link which was previously attached to the robot link through a fixed joint. + * @param model_name name that identifies model in gazebo (that are already spawned in gazebo) + * @param model_link_name name of a the link in the model that is attached to the robot + * @return true if success, false otherwise + */ + virtual bool detachUnscoped(const std::string& model_name, const std::string& model_link_name); + + // help method + virtual std::vector help(const std::string& functionName = "--all"); + + // read from ConnectionReader + bool read(yarp::os::ConnectionReader& connection) override; }; -#endif +} // namespace GazeboYarpPlugins + +#endif // YARP_THRIFT_GENERATOR_SERVICE_LINKATTACHERSERVER_H diff --git a/thrift/linkattacher/autogenerated/linkattacher_rpc_thrift.cmake b/thrift/linkattacher/autogenerated/linkattacher_rpc_thrift.cmake index 719169e0f..94d31abdb 100644 --- a/thrift/linkattacher/autogenerated/linkattacher_rpc_thrift.cmake +++ b/thrift/linkattacher/autogenerated/linkattacher_rpc_thrift.cmake @@ -1,4 +1,4 @@ -# Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) +# Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) # All rights reserved. # # This software may be modified and distributed under the terms of the diff --git a/thrift/linkattacher/autogenerated/src/LinkAttacherServer.cpp b/thrift/linkattacher/autogenerated/src/LinkAttacherServer.cpp index 2b612c78e..2e2ff4605 100644 --- a/thrift/linkattacher/autogenerated/src/LinkAttacherServer.cpp +++ b/thrift/linkattacher/autogenerated/src/LinkAttacherServer.cpp @@ -1,320 +1,413 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. #include + #include namespace GazeboYarpPlugins { - -class LinkAttacherServer_enableGravity : public yarp::os::Portable { +class LinkAttacherServer_enableGravity : + public yarp::os::Portable +{ public: - std::string model_name; - bool enable; - bool _return; - void init(const std::string& model_name, const bool enable); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + std::string model_name; + bool enable; + bool _return; + void init(const std::string& model_name, const bool enable); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class LinkAttacherServer_attachUnscoped : public yarp::os::Portable { -public: - std::string model_name; - std::string model_link_name; - std::string robot_name; - std::string robot_link_name; - bool _return; - void init(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; - -class LinkAttacherServer_detachUnscoped : public yarp::os::Portable { -public: - std::string model_name; - std::string model_link_name; - bool _return; - void init(const std::string& model_name, const std::string& model_link_name); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; - -bool LinkAttacherServer_enableGravity::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(3)) return false; - if (!writer.writeTag("enableGravity",1,1)) return false; - if (!writer.writeString(model_name)) return false; - if (!writer.writeBool(enable)) return false; - return true; -} - -bool LinkAttacherServer_enableGravity::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; -} - -void LinkAttacherServer_enableGravity::init(const std::string& model_name, const bool enable) { - _return = false; - this->model_name = model_name; - this->enable = enable; +bool LinkAttacherServer_enableGravity::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeTag("enableGravity", 1, 1)) { + return false; + } + if (!writer.writeString(model_name)) { + return false; + } + if (!writer.writeBool(enable)) { + return false; + } + return true; } -bool LinkAttacherServer_attachUnscoped::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(5)) return false; - if (!writer.writeTag("attachUnscoped",1,1)) return false; - if (!writer.writeString(model_name)) return false; - if (!writer.writeString(model_link_name)) return false; - if (!writer.writeString(robot_name)) return false; - if (!writer.writeString(robot_link_name)) return false; - return true; +bool LinkAttacherServer_enableGravity::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readBool(_return)) { + reader.fail(); + return false; + } + return true; } -bool LinkAttacherServer_attachUnscoped::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; +void LinkAttacherServer_enableGravity::init(const std::string& model_name, const bool enable) +{ + _return = false; + this->model_name = model_name; + this->enable = enable; } -void LinkAttacherServer_attachUnscoped::init(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name) { - _return = false; - this->model_name = model_name; - this->model_link_name = model_link_name; - this->robot_name = robot_name; - this->robot_link_name = robot_link_name; -} +class LinkAttacherServer_attachUnscoped : + public yarp::os::Portable +{ +public: + std::string model_name; + std::string model_link_name; + std::string robot_name; + std::string robot_link_name; + bool _return; + void init(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; -bool LinkAttacherServer_detachUnscoped::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(3)) return false; - if (!writer.writeTag("detachUnscoped",1,1)) return false; - if (!writer.writeString(model_name)) return false; - if (!writer.writeString(model_link_name)) return false; - return true; +bool LinkAttacherServer_attachUnscoped::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(5)) { + return false; + } + if (!writer.writeTag("attachUnscoped", 1, 1)) { + return false; + } + if (!writer.writeString(model_name)) { + return false; + } + if (!writer.writeString(model_link_name)) { + return false; + } + if (!writer.writeString(robot_name)) { + return false; + } + if (!writer.writeString(robot_link_name)) { + return false; + } + return true; } -bool LinkAttacherServer_detachUnscoped::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; +bool LinkAttacherServer_attachUnscoped::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readBool(_return)) { + reader.fail(); + return false; + } + return true; } -void LinkAttacherServer_detachUnscoped::init(const std::string& model_name, const std::string& model_link_name) { - _return = false; - this->model_name = model_name; - this->model_link_name = model_link_name; +void LinkAttacherServer_attachUnscoped::init(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name) +{ + _return = false; + this->model_name = model_name; + this->model_link_name = model_link_name; + this->robot_name = robot_name; + this->robot_link_name = robot_link_name; } -LinkAttacherServer::LinkAttacherServer() { - yarp().setOwner(*this); -} -bool LinkAttacherServer::enableGravity(const std::string& model_name, const bool enable) { - bool _return = false; - LinkAttacherServer_enableGravity helper; - helper.init(model_name,enable); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool LinkAttacherServer::enableGravity(const std::string& model_name, const bool enable)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool LinkAttacherServer::attachUnscoped(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name) { - bool _return = false; - LinkAttacherServer_attachUnscoped helper; - helper.init(model_name,model_link_name,robot_name,robot_link_name); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool LinkAttacherServer::attachUnscoped(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool LinkAttacherServer::detachUnscoped(const std::string& model_name, const std::string& model_link_name) { - bool _return = false; - LinkAttacherServer_detachUnscoped helper; - helper.init(model_name,model_link_name); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool LinkAttacherServer::detachUnscoped(const std::string& model_name, const std::string& model_link_name)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} +class LinkAttacherServer_detachUnscoped : + public yarp::os::Portable +{ +public: + std::string model_name; + std::string model_link_name; + bool _return; + void init(const std::string& model_name, const std::string& model_link_name); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; -bool LinkAttacherServer::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - reader.expectAccept(); - if (!reader.readListHeader()) { reader.fail(); return false; } - std::string tag = reader.readTag(); - bool direct = (tag=="__direct__"); - if (direct) tag = reader.readTag(); - while (!reader.isError()) { - // TODO: use quick lookup, this is just a test - if (tag == "enableGravity") { - std::string model_name; - bool enable; - if (!reader.readString(model_name)) { - reader.fail(); +bool LinkAttacherServer_detachUnscoped::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(3)) { return false; - } - if (!reader.readBool(enable)) { - reader.fail(); - return false; - } - bool _return; - _return = enableGravity(model_name,enable); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; } - if (tag == "attachUnscoped") { - std::string model_name; - std::string model_link_name; - std::string robot_name; - std::string robot_link_name; - if (!reader.readString(model_name)) { - reader.fail(); + if (!writer.writeTag("detachUnscoped", 1, 1)) { return false; - } - if (!reader.readString(model_link_name)) { - reader.fail(); - return false; - } - if (!reader.readString(robot_name)) { - reader.fail(); + } + if (!writer.writeString(model_name)) { return false; - } - if (!reader.readString(robot_link_name)) { - reader.fail(); + } + if (!writer.writeString(model_link_name)) { return false; - } - bool _return; - _return = attachUnscoped(model_name,model_link_name,robot_name,robot_link_name); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; } - if (tag == "detachUnscoped") { - std::string model_name; - std::string model_link_name; - if (!reader.readString(model_name)) { - reader.fail(); + return true; +} + +bool LinkAttacherServer_detachUnscoped::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { return false; - } - if (!reader.readString(model_link_name)) { + } + if (!reader.readBool(_return)) { reader.fail(); return false; - } - bool _return; - _return = detachUnscoped(model_name,model_link_name); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; } - if (tag == "help") { - std::string functionName; - if (!reader.readString(functionName)) { - functionName = "--all"; - } - std::vector _return=help(functionName); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("many",1, 0)) return false; - if (!writer.writeListBegin(BOTTLE_TAG_INT32, static_cast(_return.size()))) return false; - std::vector ::iterator _iterHelp; - for (_iterHelp = _return.begin(); _iterHelp != _return.end(); ++_iterHelp) - { - if (!writer.writeString(*_iterHelp)) return false; - } - if (!writer.writeListEnd()) return false; - } - reader.accept(); - return true; + return true; +} + +void LinkAttacherServer_detachUnscoped::init(const std::string& model_name, const std::string& model_link_name) +{ + _return = false; + this->model_name = model_name; + this->model_link_name = model_link_name; +} + +// Constructor +LinkAttacherServer::LinkAttacherServer() +{ + yarp().setOwner(*this); +} + +bool LinkAttacherServer::enableGravity(const std::string& model_name, const bool enable) +{ + bool _return = false; + LinkAttacherServer_enableGravity helper; + helper.init(model_name,enable); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool LinkAttacherServer::enableGravity(const std::string& model_name, const bool enable)"); } - if (reader.noMore()) { reader.fail(); return false; } - std::string next_tag = reader.readTag(); - if (next_tag=="") break; - tag = tag + "_" + next_tag; - } - return false; + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; } -std::vector LinkAttacherServer::help(const std::string& functionName) { - bool showAll=(functionName=="--all"); - std::vector helpString; - if(showAll) { - helpString.push_back("*** Available commands:"); - helpString.push_back("enableGravity"); - helpString.push_back("attachUnscoped"); - helpString.push_back("detachUnscoped"); - helpString.push_back("help"); - } - else { - if (functionName=="enableGravity") { - helpString.push_back("bool enableGravity(const std::string& model_name, const bool enable) "); - helpString.push_back("Enable/disables gravity for a model "); - helpString.push_back("@param model_name name that identifies model in gazebo (that are already spawned in gazebo) "); - helpString.push_back("@param enable 1 to enable gravity, 0 otherwise "); - helpString.push_back("@return returns true or false on success failure "); +bool LinkAttacherServer::attachUnscoped(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name) +{ + bool _return = false; + LinkAttacherServer_attachUnscoped helper; + helper.init(model_name,model_link_name,robot_name,robot_link_name); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool LinkAttacherServer::attachUnscoped(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name)"); } - if (functionName=="attachUnscoped") { - helpString.push_back("bool attachUnscoped(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name) "); - helpString.push_back("Attach any link of the models spawned in gazebo to a link of the robot. "); - helpString.push_back("@param model_name name that identifies model in gazebo (that are already spawned in gazebo) "); - helpString.push_back("@param model_link_name name of a the link in the model you want to attach to the robot "); - helpString.push_back("@param robot_name name of the robot "); - helpString.push_back("@param robot_link_name name of the robot link to which you want to attached the model link "); - helpString.push_back("@return true if success, false otherwise "); + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool LinkAttacherServer::detachUnscoped(const std::string& model_name, const std::string& model_link_name) +{ + bool _return = false; + LinkAttacherServer_detachUnscoped helper; + helper.init(model_name,model_link_name); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool LinkAttacherServer::detachUnscoped(const std::string& model_name, const std::string& model_link_name)"); } - if (functionName=="detachUnscoped") { - helpString.push_back("bool detachUnscoped(const std::string& model_name, const std::string& model_link_name) "); - helpString.push_back("Detach a previously attached model link. "); - helpString.push_back("@param model_name name that identifies model in gazebo (that are already spawned in gazebo) "); - helpString.push_back("@param model_link_name name of a the link in the model that is attached to the robot "); - helpString.push_back("@return true if success, false otherwise "); + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +// help method +std::vector LinkAttacherServer::help(const std::string& functionName) +{ + bool showAll = (functionName == "--all"); + std::vector helpString; + if (showAll) { + helpString.emplace_back("*** Available commands:"); + helpString.emplace_back("enableGravity"); + helpString.emplace_back("attachUnscoped"); + helpString.emplace_back("detachUnscoped"); + helpString.emplace_back("help"); + } else { + if (functionName == "enableGravity") { + helpString.emplace_back("bool enableGravity(const std::string& model_name, const bool enable) "); + helpString.emplace_back("Enable/disables gravity for a model "); + helpString.emplace_back("@param model_name name that identifies model in gazebo (that are already spawned in gazebo) "); + helpString.emplace_back("@param enable 1 to enable gravity, 0 otherwise "); + helpString.emplace_back("@return returns true or false on success failure "); + } + if (functionName == "attachUnscoped") { + helpString.emplace_back("bool attachUnscoped(const std::string& model_name, const std::string& model_link_name, const std::string& robot_name, const std::string& robot_link_name) "); + helpString.emplace_back("Attach any link of the models spawned in gazebo to a link of the robot using a fixed joint. "); + helpString.emplace_back("@param model_name name that identifies model in gazebo (that are already spawned in gazebo) "); + helpString.emplace_back("@param model_link_name name of a the link in the model you want to attach to the robot "); + helpString.emplace_back("@param robot_name name of the robot "); + helpString.emplace_back("@param robot_link_name name of the robot link to which you want to attached the model link "); + helpString.emplace_back("@return true if success, false otherwise "); + } + if (functionName == "detachUnscoped") { + helpString.emplace_back("bool detachUnscoped(const std::string& model_name, const std::string& model_link_name) "); + helpString.emplace_back("Detach the model link which was previously attached to the robot link through a fixed joint. "); + helpString.emplace_back("@param model_name name that identifies model in gazebo (that are already spawned in gazebo) "); + helpString.emplace_back("@param model_link_name name of a the link in the model that is attached to the robot "); + helpString.emplace_back("@return true if success, false otherwise "); + } + if (functionName == "help") { + helpString.emplace_back("std::vector help(const std::string& functionName = \"--all\")"); + helpString.emplace_back("Return list of available commands, or help message for a specific function"); + helpString.emplace_back("@param functionName name of command for which to get a detailed description. If none or '--all' is provided, print list of available commands"); + helpString.emplace_back("@return list of strings (one string per line)"); + } } - if (functionName=="help") { - helpString.push_back("std::vector help(const std::string& functionName=\"--all\")"); - helpString.push_back("Return list of available commands, or help message for a specific function"); - helpString.push_back("@param functionName name of command for which to get a detailed description. If none or '--all' is provided, print list of available commands"); - helpString.push_back("@return list of strings (one string per line)"); + if (helpString.empty()) { + helpString.emplace_back("Command not found"); } - } - if ( helpString.empty()) helpString.push_back("Command not found"); - return helpString; + return helpString; } -} // namespace +// read from ConnectionReader +bool LinkAttacherServer::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + reader.expectAccept(); + if (!reader.readListHeader()) { + reader.fail(); + return false; + } + + std::string tag = reader.readTag(); + bool direct = (tag == "__direct__"); + if (direct) { + tag = reader.readTag(); + } + while (!reader.isError()) { + if (tag == "enableGravity") { + std::string model_name; + bool enable; + if (!reader.readString(model_name)) { + reader.fail(); + return false; + } + if (!reader.readBool(enable)) { + reader.fail(); + return false; + } + bool _return; + _return = enableGravity(model_name,enable); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "attachUnscoped") { + std::string model_name; + std::string model_link_name; + std::string robot_name; + std::string robot_link_name; + if (!reader.readString(model_name)) { + reader.fail(); + return false; + } + if (!reader.readString(model_link_name)) { + reader.fail(); + return false; + } + if (!reader.readString(robot_name)) { + reader.fail(); + return false; + } + if (!reader.readString(robot_link_name)) { + reader.fail(); + return false; + } + bool _return; + _return = attachUnscoped(model_name,model_link_name,robot_name,robot_link_name); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "detachUnscoped") { + std::string model_name; + std::string model_link_name; + if (!reader.readString(model_name)) { + reader.fail(); + return false; + } + if (!reader.readString(model_link_name)) { + reader.fail(); + return false; + } + bool _return; + _return = detachUnscoped(model_name,model_link_name); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "help") { + std::string functionName; + if (!reader.readString(functionName)) { + functionName = "--all"; + } + auto _return = help(functionName); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(2)) { + return false; + } + if (!writer.writeTag("many", 1, 0)) { + return false; + } + if (!writer.writeListBegin(BOTTLE_TAG_INT32, static_cast(_return.size()))) { + return false; + } + for (const auto& _ret : _return) { + if (!writer.writeString(_ret)) { + return false; + } + } + if (!writer.writeListEnd()) { + return false; + } + } + reader.accept(); + return true; + } + if (reader.noMore()) { + reader.fail(); + return false; + } + std::string next_tag = reader.readTag(); + if (next_tag == "") { + break; + } + tag.append("_").append(next_tag); + } + return false; +} +} // namespace GazeboYarpPlugins diff --git a/thrift/worldinterface/autogenerated/include/Color.h b/thrift/worldinterface/autogenerated/include/Color.h index 89bd89452..3064d7f14 100644 --- a/thrift/worldinterface/autogenerated/include/Color.h +++ b/thrift/worldinterface/autogenerated/include/Color.h @@ -1,213 +1,185 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. -#ifndef YARP_THRIFT_GENERATOR_STRUCT_Color -#define YARP_THRIFT_GENERATOR_STRUCT_Color +#ifndef YARP_THRIFT_GENERATOR_STRUCT_COLOR_H +#define YARP_THRIFT_GENERATOR_STRUCT_COLOR_H #include #include namespace GazeboYarpPlugins { - class Color; -} - -class GazeboYarpPlugins::Color : public yarp::os::idl::WirePortable { +class Color : + public yarp::os::idl::WirePortable +{ public: - // Fields - std::int16_t r; - std::int16_t g; - std::int16_t b; - - // Default constructor - Color() : r(0), g(0), b(0) { - } - - // Constructor with field values - Color(const std::int16_t r,const std::int16_t g,const std::int16_t b) : r(r), g(g), b(b) { - } - - // Copy constructor - Color(const Color& __alt) : WirePortable(__alt) { - this->r = __alt.r; - this->g = __alt.g; - this->b = __alt.b; - } - - // Assignment operator - const Color& operator = (const Color& __alt) { - this->r = __alt.r; - this->g = __alt.g; - this->b = __alt.b; - return *this; - } - - // read and write structure on a connection - bool read(yarp::os::idl::WireReader& reader) override; - bool read(yarp::os::ConnectionReader& connection) override; - bool write(const yarp::os::idl::WireWriter& writer) const override; - bool write(yarp::os::ConnectionWriter& connection) const override; + // Fields + std::int16_t r; + std::int16_t g; + std::int16_t b; -private: - bool write_r(const yarp::os::idl::WireWriter& writer) const; - bool nested_write_r(const yarp::os::idl::WireWriter& writer) const; - bool write_g(const yarp::os::idl::WireWriter& writer) const; - bool nested_write_g(const yarp::os::idl::WireWriter& writer) const; - bool write_b(const yarp::os::idl::WireWriter& writer) const; - bool nested_write_b(const yarp::os::idl::WireWriter& writer) const; - bool read_r(yarp::os::idl::WireReader& reader); - bool nested_read_r(yarp::os::idl::WireReader& reader); - bool read_g(yarp::os::idl::WireReader& reader); - bool nested_read_g(yarp::os::idl::WireReader& reader); - bool read_b(yarp::os::idl::WireReader& reader); - bool nested_read_b(yarp::os::idl::WireReader& reader); + // Default constructor + Color(); -public: + // Constructor with field values + Color(const std::int16_t r, + const std::int16_t g, + const std::int16_t b); - std::string toString() const; - - // if you want to serialize this class without nesting, use this helper - typedef yarp::os::idl::Unwrapped unwrapped; - - class Editor : public yarp::os::Wire, public yarp::os::PortWriter { - public: - - Editor() { - group = 0; - obj_owned = true; - obj = new Color; - dirty_flags(false); - yarp().setOwner(*this); - } - - Editor(Color& obj) { - group = 0; - obj_owned = false; - edit(obj,false); - yarp().setOwner(*this); - } - - bool edit(Color& obj, bool dirty = true) { - if (obj_owned) delete this->obj; - this->obj = &obj; - obj_owned = false; - dirty_flags(dirty); - return true; - } - - virtual ~Editor() { - if (obj_owned) delete obj; - } - - bool isValid() const { - return obj!=0/*NULL*/; - } - - Color& state() { return *obj; } - - void begin() { group++; } - - void end() { - group--; - if (group==0&&is_dirty) communicate(); - } - void set_r(const std::int16_t r) { - will_set_r(); - obj->r = r; - mark_dirty_r(); - communicate(); - did_set_r(); - } - void set_g(const std::int16_t g) { - will_set_g(); - obj->g = g; - mark_dirty_g(); - communicate(); - did_set_g(); - } - void set_b(const std::int16_t b) { - will_set_b(); - obj->b = b; - mark_dirty_b(); - communicate(); - did_set_b(); - } - std::int16_t get_r() { - return obj->r; - } - std::int16_t get_g() { - return obj->g; - } - std::int16_t get_b() { - return obj->b; - } - virtual bool will_set_r() { return true; } - virtual bool will_set_g() { return true; } - virtual bool will_set_b() { return true; } - virtual bool did_set_r() { return true; } - virtual bool did_set_g() { return true; } - virtual bool did_set_b() { return true; } - void clean() { - dirty_flags(false); - } + // Read structure on a Wire + bool read(yarp::os::idl::WireReader& reader) override; + + // Read structure on a Connection bool read(yarp::os::ConnectionReader& connection) override; + + // Write structure on a Wire + bool write(const yarp::os::idl::WireWriter& writer) const override; + + // Write structure on a Connection bool write(yarp::os::ConnectionWriter& connection) const override; - private: - - Color *obj; - - bool obj_owned; - int group; - - void communicate() { - if (group!=0) return; - if (yarp().canWrite()) { - yarp().write(*this); - clean(); - } - } - void mark_dirty() { - is_dirty = true; - } - void mark_dirty_r() { - if (is_dirty_r) return; - dirty_count++; - is_dirty_r = true; - mark_dirty(); - } - void mark_dirty_g() { - if (is_dirty_g) return; - dirty_count++; - is_dirty_g = true; - mark_dirty(); - } - void mark_dirty_b() { - if (is_dirty_b) return; - dirty_count++; - is_dirty_b = true; - mark_dirty(); - } - void dirty_flags(bool flag) { - is_dirty = flag; - is_dirty_r = flag; - is_dirty_g = flag; - is_dirty_b = flag; - dirty_count = flag ? 3 : 0; - } - bool is_dirty; - int dirty_count; - bool is_dirty_r; - bool is_dirty_g; - bool is_dirty_b; - }; + + // Convert to a printable string + std::string toString() const; + + // If you want to serialize this class without nesting, use this helper + typedef yarp::os::idl::Unwrapped unwrapped; + + class Editor : + public yarp::os::Wire, + public yarp::os::PortWriter + { + public: + // Editor: default constructor + Editor(); + + // Editor: constructor with base class + Editor(Color& obj); + + // Editor: destructor + ~Editor() override; + + // Editor: Deleted constructors and operator= + Editor(const Editor& rhs) = delete; + Editor(Editor&& rhs) = delete; + Editor& operator=(const Editor& rhs) = delete; + Editor& operator=(Editor&& rhs) = delete; + + // Editor: edit + bool edit(Color& obj, bool dirty = true); + + // Editor: validity check + bool isValid() const; + + // Editor: state + Color& state(); + + // Editor: start editing + void start_editing(); + +#ifndef YARP_NO_DEPRECATED // Since YARP 3.2 + YARP_DEPRECATED_MSG("Use start_editing() instead") + void begin() + { + start_editing(); + } +#endif // YARP_NO_DEPRECATED + + // Editor: stop editing + void stop_editing(); + +#ifndef YARP_NO_DEPRECATED // Since YARP 3.2 + YARP_DEPRECATED_MSG("Use stop_editing() instead") + void end() + { + stop_editing(); + } +#endif // YARP_NO_DEPRECATED + + // Editor: r field + void set_r(const std::int16_t r); + std::int16_t get_r() const; + virtual bool will_set_r(); + virtual bool did_set_r(); + + // Editor: g field + void set_g(const std::int16_t g); + std::int16_t get_g() const; + virtual bool will_set_g(); + virtual bool did_set_g(); + + // Editor: b field + void set_b(const std::int16_t b); + std::int16_t get_b() const; + virtual bool will_set_b(); + virtual bool did_set_b(); + + // Editor: clean + void clean(); + + // Editor: read + bool read(yarp::os::ConnectionReader& connection) override; + + // Editor: write + bool write(yarp::os::ConnectionWriter& connection) const override; + + private: + // Editor: state + Color* obj; + bool obj_owned; + int group; + + // Editor: dirty variables + bool is_dirty; + bool is_dirty_r; + bool is_dirty_g; + bool is_dirty_b; + int dirty_count; + + // Editor: send if possible + void communicate(); + + // Editor: mark dirty overall + void mark_dirty(); + + // Editor: mark dirty single fields + void mark_dirty_r(); + void mark_dirty_g(); + void mark_dirty_b(); + + // Editor: dirty_flags + void dirty_flags(bool flag); + }; + +private: + // read/write r field + bool read_r(yarp::os::idl::WireReader& reader); + bool write_r(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_r(yarp::os::idl::WireReader& reader); + bool nested_write_r(const yarp::os::idl::WireWriter& writer) const; + + // read/write g field + bool read_g(yarp::os::idl::WireReader& reader); + bool write_g(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_g(yarp::os::idl::WireReader& reader); + bool nested_write_g(const yarp::os::idl::WireWriter& writer) const; + + // read/write b field + bool read_b(yarp::os::idl::WireReader& reader); + bool write_b(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_b(yarp::os::idl::WireReader& reader); + bool nested_write_b(const yarp::os::idl::WireWriter& writer) const; }; -#endif +} // namespace GazeboYarpPlugins + +#endif // YARP_THRIFT_GENERATOR_STRUCT_COLOR_H diff --git a/thrift/worldinterface/autogenerated/include/Pose.h b/thrift/worldinterface/autogenerated/include/Pose.h index f9bb1abfe..fb7cdc65b 100644 --- a/thrift/worldinterface/autogenerated/include/Pose.h +++ b/thrift/worldinterface/autogenerated/include/Pose.h @@ -1,294 +1,233 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. -#ifndef YARP_THRIFT_GENERATOR_STRUCT_Pose -#define YARP_THRIFT_GENERATOR_STRUCT_Pose +#ifndef YARP_THRIFT_GENERATOR_STRUCT_POSE_H +#define YARP_THRIFT_GENERATOR_STRUCT_POSE_H #include #include namespace GazeboYarpPlugins { - class Pose; -} - -class GazeboYarpPlugins::Pose : public yarp::os::idl::WirePortable { +class Pose : + public yarp::os::idl::WirePortable +{ public: - // Fields - double x; - double y; - double z; - double roll; - double pitch; - double yaw; - - // Default constructor - Pose() : x(0), y(0), z(0), roll(0), pitch(0), yaw(0) { - } - - // Constructor with field values - Pose(const double x,const double y,const double z,const double roll,const double pitch,const double yaw) : x(x), y(y), z(z), roll(roll), pitch(pitch), yaw(yaw) { - } - - // Copy constructor - Pose(const Pose& __alt) : WirePortable(__alt) { - this->x = __alt.x; - this->y = __alt.y; - this->z = __alt.z; - this->roll = __alt.roll; - this->pitch = __alt.pitch; - this->yaw = __alt.yaw; - } - - // Assignment operator - const Pose& operator = (const Pose& __alt) { - this->x = __alt.x; - this->y = __alt.y; - this->z = __alt.z; - this->roll = __alt.roll; - this->pitch = __alt.pitch; - this->yaw = __alt.yaw; - return *this; - } - - // read and write structure on a connection - bool read(yarp::os::idl::WireReader& reader) override; - bool read(yarp::os::ConnectionReader& connection) override; - bool write(const yarp::os::idl::WireWriter& writer) const override; - bool write(yarp::os::ConnectionWriter& connection) const override; - -private: - bool write_x(const yarp::os::idl::WireWriter& writer) const; - bool nested_write_x(const yarp::os::idl::WireWriter& writer) const; - bool write_y(const yarp::os::idl::WireWriter& writer) const; - bool nested_write_y(const yarp::os::idl::WireWriter& writer) const; - bool write_z(const yarp::os::idl::WireWriter& writer) const; - bool nested_write_z(const yarp::os::idl::WireWriter& writer) const; - bool write_roll(const yarp::os::idl::WireWriter& writer) const; - bool nested_write_roll(const yarp::os::idl::WireWriter& writer) const; - bool write_pitch(const yarp::os::idl::WireWriter& writer) const; - bool nested_write_pitch(const yarp::os::idl::WireWriter& writer) const; - bool write_yaw(const yarp::os::idl::WireWriter& writer) const; - bool nested_write_yaw(const yarp::os::idl::WireWriter& writer) const; - bool read_x(yarp::os::idl::WireReader& reader); - bool nested_read_x(yarp::os::idl::WireReader& reader); - bool read_y(yarp::os::idl::WireReader& reader); - bool nested_read_y(yarp::os::idl::WireReader& reader); - bool read_z(yarp::os::idl::WireReader& reader); - bool nested_read_z(yarp::os::idl::WireReader& reader); - bool read_roll(yarp::os::idl::WireReader& reader); - bool nested_read_roll(yarp::os::idl::WireReader& reader); - bool read_pitch(yarp::os::idl::WireReader& reader); - bool nested_read_pitch(yarp::os::idl::WireReader& reader); - bool read_yaw(yarp::os::idl::WireReader& reader); - bool nested_read_yaw(yarp::os::idl::WireReader& reader); + // Fields + double x; + double y; + double z; + double roll; + double pitch; + double yaw; + + // Default constructor + Pose(); + + // Constructor with field values + Pose(const double x, + const double y, + const double z, + const double roll, + const double pitch, + const double yaw); + + // Read structure on a Wire + bool read(yarp::os::idl::WireReader& reader) override; + + // Read structure on a Connection + bool read(yarp::os::ConnectionReader& connection) override; -public: + // Write structure on a Wire + bool write(const yarp::os::idl::WireWriter& writer) const override; - std::string toString() const; - - // if you want to serialize this class without nesting, use this helper - typedef yarp::os::idl::Unwrapped unwrapped; - - class Editor : public yarp::os::Wire, public yarp::os::PortWriter { - public: - - Editor() { - group = 0; - obj_owned = true; - obj = new Pose; - dirty_flags(false); - yarp().setOwner(*this); - } - - Editor(Pose& obj) { - group = 0; - obj_owned = false; - edit(obj,false); - yarp().setOwner(*this); - } - - bool edit(Pose& obj, bool dirty = true) { - if (obj_owned) delete this->obj; - this->obj = &obj; - obj_owned = false; - dirty_flags(dirty); - return true; - } - - virtual ~Editor() { - if (obj_owned) delete obj; - } - - bool isValid() const { - return obj!=0/*NULL*/; - } - - Pose& state() { return *obj; } - - void begin() { group++; } - - void end() { - group--; - if (group==0&&is_dirty) communicate(); - } - void set_x(const double x) { - will_set_x(); - obj->x = x; - mark_dirty_x(); - communicate(); - did_set_x(); - } - void set_y(const double y) { - will_set_y(); - obj->y = y; - mark_dirty_y(); - communicate(); - did_set_y(); - } - void set_z(const double z) { - will_set_z(); - obj->z = z; - mark_dirty_z(); - communicate(); - did_set_z(); - } - void set_roll(const double roll) { - will_set_roll(); - obj->roll = roll; - mark_dirty_roll(); - communicate(); - did_set_roll(); - } - void set_pitch(const double pitch) { - will_set_pitch(); - obj->pitch = pitch; - mark_dirty_pitch(); - communicate(); - did_set_pitch(); - } - void set_yaw(const double yaw) { - will_set_yaw(); - obj->yaw = yaw; - mark_dirty_yaw(); - communicate(); - did_set_yaw(); - } - double get_x() { - return obj->x; - } - double get_y() { - return obj->y; - } - double get_z() { - return obj->z; - } - double get_roll() { - return obj->roll; - } - double get_pitch() { - return obj->pitch; - } - double get_yaw() { - return obj->yaw; - } - virtual bool will_set_x() { return true; } - virtual bool will_set_y() { return true; } - virtual bool will_set_z() { return true; } - virtual bool will_set_roll() { return true; } - virtual bool will_set_pitch() { return true; } - virtual bool will_set_yaw() { return true; } - virtual bool did_set_x() { return true; } - virtual bool did_set_y() { return true; } - virtual bool did_set_z() { return true; } - virtual bool did_set_roll() { return true; } - virtual bool did_set_pitch() { return true; } - virtual bool did_set_yaw() { return true; } - void clean() { - dirty_flags(false); - } - bool read(yarp::os::ConnectionReader& connection) override; + // Write structure on a Connection bool write(yarp::os::ConnectionWriter& connection) const override; - private: - - Pose *obj; - - bool obj_owned; - int group; - - void communicate() { - if (group!=0) return; - if (yarp().canWrite()) { - yarp().write(*this); - clean(); - } - } - void mark_dirty() { - is_dirty = true; - } - void mark_dirty_x() { - if (is_dirty_x) return; - dirty_count++; - is_dirty_x = true; - mark_dirty(); - } - void mark_dirty_y() { - if (is_dirty_y) return; - dirty_count++; - is_dirty_y = true; - mark_dirty(); - } - void mark_dirty_z() { - if (is_dirty_z) return; - dirty_count++; - is_dirty_z = true; - mark_dirty(); - } - void mark_dirty_roll() { - if (is_dirty_roll) return; - dirty_count++; - is_dirty_roll = true; - mark_dirty(); - } - void mark_dirty_pitch() { - if (is_dirty_pitch) return; - dirty_count++; - is_dirty_pitch = true; - mark_dirty(); - } - void mark_dirty_yaw() { - if (is_dirty_yaw) return; - dirty_count++; - is_dirty_yaw = true; - mark_dirty(); - } - void dirty_flags(bool flag) { - is_dirty = flag; - is_dirty_x = flag; - is_dirty_y = flag; - is_dirty_z = flag; - is_dirty_roll = flag; - is_dirty_pitch = flag; - is_dirty_yaw = flag; - dirty_count = flag ? 6 : 0; - } - bool is_dirty; - int dirty_count; - bool is_dirty_x; - bool is_dirty_y; - bool is_dirty_z; - bool is_dirty_roll; - bool is_dirty_pitch; - bool is_dirty_yaw; - }; + + // Convert to a printable string + std::string toString() const; + + // If you want to serialize this class without nesting, use this helper + typedef yarp::os::idl::Unwrapped unwrapped; + + class Editor : + public yarp::os::Wire, + public yarp::os::PortWriter + { + public: + // Editor: default constructor + Editor(); + + // Editor: constructor with base class + Editor(Pose& obj); + + // Editor: destructor + ~Editor() override; + + // Editor: Deleted constructors and operator= + Editor(const Editor& rhs) = delete; + Editor(Editor&& rhs) = delete; + Editor& operator=(const Editor& rhs) = delete; + Editor& operator=(Editor&& rhs) = delete; + + // Editor: edit + bool edit(Pose& obj, bool dirty = true); + + // Editor: validity check + bool isValid() const; + + // Editor: state + Pose& state(); + + // Editor: start editing + void start_editing(); + +#ifndef YARP_NO_DEPRECATED // Since YARP 3.2 + YARP_DEPRECATED_MSG("Use start_editing() instead") + void begin() + { + start_editing(); + } +#endif // YARP_NO_DEPRECATED + + // Editor: stop editing + void stop_editing(); + +#ifndef YARP_NO_DEPRECATED // Since YARP 3.2 + YARP_DEPRECATED_MSG("Use stop_editing() instead") + void end() + { + stop_editing(); + } +#endif // YARP_NO_DEPRECATED + + // Editor: x field + void set_x(const double x); + double get_x() const; + virtual bool will_set_x(); + virtual bool did_set_x(); + + // Editor: y field + void set_y(const double y); + double get_y() const; + virtual bool will_set_y(); + virtual bool did_set_y(); + + // Editor: z field + void set_z(const double z); + double get_z() const; + virtual bool will_set_z(); + virtual bool did_set_z(); + + // Editor: roll field + void set_roll(const double roll); + double get_roll() const; + virtual bool will_set_roll(); + virtual bool did_set_roll(); + + // Editor: pitch field + void set_pitch(const double pitch); + double get_pitch() const; + virtual bool will_set_pitch(); + virtual bool did_set_pitch(); + + // Editor: yaw field + void set_yaw(const double yaw); + double get_yaw() const; + virtual bool will_set_yaw(); + virtual bool did_set_yaw(); + + // Editor: clean + void clean(); + + // Editor: read + bool read(yarp::os::ConnectionReader& connection) override; + + // Editor: write + bool write(yarp::os::ConnectionWriter& connection) const override; + + private: + // Editor: state + Pose* obj; + bool obj_owned; + int group; + + // Editor: dirty variables + bool is_dirty; + bool is_dirty_x; + bool is_dirty_y; + bool is_dirty_z; + bool is_dirty_roll; + bool is_dirty_pitch; + bool is_dirty_yaw; + int dirty_count; + + // Editor: send if possible + void communicate(); + + // Editor: mark dirty overall + void mark_dirty(); + + // Editor: mark dirty single fields + void mark_dirty_x(); + void mark_dirty_y(); + void mark_dirty_z(); + void mark_dirty_roll(); + void mark_dirty_pitch(); + void mark_dirty_yaw(); + + // Editor: dirty_flags + void dirty_flags(bool flag); + }; + +private: + // read/write x field + bool read_x(yarp::os::idl::WireReader& reader); + bool write_x(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_x(yarp::os::idl::WireReader& reader); + bool nested_write_x(const yarp::os::idl::WireWriter& writer) const; + + // read/write y field + bool read_y(yarp::os::idl::WireReader& reader); + bool write_y(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_y(yarp::os::idl::WireReader& reader); + bool nested_write_y(const yarp::os::idl::WireWriter& writer) const; + + // read/write z field + bool read_z(yarp::os::idl::WireReader& reader); + bool write_z(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_z(yarp::os::idl::WireReader& reader); + bool nested_write_z(const yarp::os::idl::WireWriter& writer) const; + + // read/write roll field + bool read_roll(yarp::os::idl::WireReader& reader); + bool write_roll(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_roll(yarp::os::idl::WireReader& reader); + bool nested_write_roll(const yarp::os::idl::WireWriter& writer) const; + + // read/write pitch field + bool read_pitch(yarp::os::idl::WireReader& reader); + bool write_pitch(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_pitch(yarp::os::idl::WireReader& reader); + bool nested_write_pitch(const yarp::os::idl::WireWriter& writer) const; + + // read/write yaw field + bool read_yaw(yarp::os::idl::WireReader& reader); + bool write_yaw(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_yaw(yarp::os::idl::WireReader& reader); + bool nested_write_yaw(const yarp::os::idl::WireWriter& writer) const; }; -#endif +} // namespace GazeboYarpPlugins + +#endif // YARP_THRIFT_GENERATOR_STRUCT_POSE_H diff --git a/thrift/worldinterface/autogenerated/include/WorldInterfaceServer.h b/thrift/worldinterface/autogenerated/include/WorldInterfaceServer.h index 798421033..e8b2aa89b 100644 --- a/thrift/worldinterface/autogenerated/include/WorldInterfaceServer.h +++ b/thrift/worldinterface/autogenerated/include/WorldInterfaceServer.h @@ -1,16 +1,18 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. -#ifndef YARP_THRIFT_GENERATOR_WorldInterfaceServer -#define YARP_THRIFT_GENERATOR_WorldInterfaceServer +#ifndef YARP_THRIFT_GENERATOR_SERVICE_WORLDINTERFACESERVER_H +#define YARP_THRIFT_GENERATOR_SERVICE_WORLDINTERFACESERVER_H #include #include @@ -18,143 +20,165 @@ #include namespace GazeboYarpPlugins { - class WorldInterfaceServer; -} - -class GazeboYarpPlugins::WorldInterfaceServer : public yarp::os::Wire { +class WorldInterfaceServer : + public yarp::os::Wire +{ public: - WorldInterfaceServer(); - /** - * Make a sphere. - * @param radius radius of the sphere [m] - * @param pose pose of the sphere [m] - * @param color color of the sphere - * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. - * @param object_name (optional) assigns a name to the object. - * @param gravity_enable (optional) enables gravity (default false) - * @param collision_enable (optional) enables collision (default true) - * @return returns a string that contains the name of the object in the world - */ - virtual std::string makeSphere(const double radius, const Pose& pose, const Color& color, const std::string& frame_name = "", const std::string& object_name = "", const bool gravity_enable = 0, const bool collision_enable = 1); - /** - * Make a box. - * @param width box width [m] - * @param height box height[m] - * @param thickness box thickness [m] - * @param pose pose of the box [m] - * @param color color of the box - * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. - * @param object_name (optional) assigns a name to the object. - * @param gravity_enable (optional) enables gravity (default false) - * @param collision_enable (optional) enables collision (default true) - * @return returns a string that contains the name of the object in the world - */ - virtual std::string makeBox(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name = "", const std::string& object_name = "", const bool gravity_enable = 0, const bool collision_enable = 1); - /** - * Make a cylinder. - * @param radius radius of the cylinder [m] - * @param length lenght of the cylinder [m] - * @param pose pose of the cylinder [m] - * @param color color of the cylinder - * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. - * @param object_name (optional) assigns a name to the object. - * @param gravity_enable (optional) enables gravity (default false) - * @param collision_enable (optional) enables collision (default true) - * @return returns a string that contains the name of the object in the world - */ - virtual std::string makeCylinder(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name = "", const std::string& object_name = "", const bool gravity_enable = 0, const bool collision_enable = 1); - /** - * Make a reference frame. - * @param size size of the frame [m] - * @param pose pose of the frame [m] - * @param color color of the frame - * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. - * @param object_name (optional) assigns a name to the object. - * @param gravity_enable (optional) enables gravity (default false) - * @param collision_enable (optional) enables collision (default true) - * @return returns a string that contains the name of the object in the world - */ - virtual std::string makeFrame(const double size, const Pose& pose, const Color& color, const std::string& frame_name = "", const std::string& object_name = "", const bool gravity_enable = 0, const bool collision_enable = 1); - /** - * Change the color of an object - * @param id object id - * @param color color of the frame - * @return returns true or false on success failure - */ - virtual bool changeColor(const std::string& id, const Color& color); - /** - * Set new object pose. - * @param id object id - * @param pose new pose - * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. - * @return returns true or false on success failure - */ - virtual bool setPose(const std::string& id, const Pose& pose, const std::string& frame_name = ""); - /** - * Enable/disables gravity for an object - * @param id object id - * @param enable 1 to enable gravity, 0 otherwise - * @return returns true or false on success failure - */ - virtual bool enableGravity(const std::string& id, const bool enable); - /** - * Enable/disables collision detection for an object - * @param id object id - * @param enable 1 to enable collision detection, 0 otherwise - * @return returns true or false on success failure - */ - virtual bool enableCollision(const std::string& id, const bool enable); - /** - * Get object pose. - * @param id string that identifies object in gazebo (returned after creation) - * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. - * @return returns value of the pose in the world reference frame - */ - virtual Pose getPose(const std::string& id, const std::string& frame_name = ""); - /** - * Load a model from file. - * @param id string that specifies the name of the model - * @return returns true/false on success failure. - */ - virtual bool loadModelFromFile(const std::string& filename); - /** - * Delete an object. - * @param id string that identifies object in gazebo (returned after creation) - * @return returns true/false on success failure. - */ - virtual bool deleteObject(const std::string& id); - /** - * Delete all objects in the world. - */ - virtual bool deleteAll(); - /** - * List id of all objects that have been added to the world. - * @return return a list of string containing the id of the objects - */ - virtual std::vector getList(); - /** - * Attach an object to a link of the robot. - * @param id string that identifies object in gazebo (returned after creation) - * @param link_name name of a link of the robot - * @return true if success, false otherwise - */ - virtual bool attach(const std::string& id, const std::string& link_name); - /** - * Detach a previously attached object. - * @param id string that identifies object in gazebo (returned after creation) - * @return true if success, false otherwise - */ - virtual bool detach(const std::string& id); - /** - * Change the names of an object. - * @param old_name string that identifies object in gazebo - * @param new_name string that will be used as new name - * @return true if success, false otherwise - */ - virtual bool rename(const std::string& old_name, const std::string& new_name); - virtual bool read(yarp::os::ConnectionReader& connection) override; - virtual std::vector help(const std::string& functionName="--all"); + // Constructor + WorldInterfaceServer(); + + /** + * Make a sphere. + * @param radius radius of the sphere [m] + * @param pose pose of the sphere [m] + * @param color color of the sphere + * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. + * @param object_name (optional) assigns a name to the object. + * @param gravity_enable (optional) enables gravity (default false) + * @param collision_enable (optional) enables collision (default true) + * @return returns a string that contains the name of the object in the world + */ + virtual std::string makeSphere(const double radius, const Pose& pose, const Color& color, const std::string& frame_name = "", const std::string& object_name = "", const bool gravity_enable = 0, const bool collision_enable = 1); + + /** + * Make a box. + * @param width box width [m] + * @param height box height[m] + * @param thickness box thickness [m] + * @param pose pose of the box [m] + * @param color color of the box + * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. + * @param object_name (optional) assigns a name to the object. + * @param gravity_enable (optional) enables gravity (default false) + * @param collision_enable (optional) enables collision (default true) + * @return returns a string that contains the name of the object in the world + */ + virtual std::string makeBox(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name = "", const std::string& object_name = "", const bool gravity_enable = 0, const bool collision_enable = 1); + + /** + * Make a cylinder. + * @param radius radius of the cylinder [m] + * @param length lenght of the cylinder [m] + * @param pose pose of the cylinder [m] + * @param color color of the cylinder + * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. + * @param object_name (optional) assigns a name to the object. + * @param gravity_enable (optional) enables gravity (default false) + * @param collision_enable (optional) enables collision (default true) + * @return returns a string that contains the name of the object in the world + */ + virtual std::string makeCylinder(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name = "", const std::string& object_name = "", const bool gravity_enable = 0, const bool collision_enable = 1); + + /** + * Make a reference frame. + * @param size size of the frame [m] + * @param pose pose of the frame [m] + * @param color color of the frame + * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. + * @param object_name (optional) assigns a name to the object. + * @param gravity_enable (optional) enables gravity (default false) + * @param collision_enable (optional) enables collision (default true) + * @return returns a string that contains the name of the object in the world + */ + virtual std::string makeFrame(const double size, const Pose& pose, const Color& color, const std::string& frame_name = "", const std::string& object_name = "", const bool gravity_enable = 0, const bool collision_enable = 1); + + /** + * Change the color of an object + * @param id object id + * @param color color of the frame + * @return returns true or false on success failure + */ + virtual bool changeColor(const std::string& id, const Color& color); + + /** + * Set new object pose. + * @param id object id + * @param pose new pose + * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. + * @return returns true or false on success failure + */ + virtual bool setPose(const std::string& id, const Pose& pose, const std::string& frame_name = ""); + + /** + * Enable/disables gravity for an object + * @param id object id + * @param enable 1 to enable gravity, 0 otherwise + * @return returns true or false on success failure + */ + virtual bool enableGravity(const std::string& id, const bool enable); + + /** + * Enable/disables collision detection for an object + * @param id object id + * @param enable 1 to enable collision detection, 0 otherwise + * @return returns true or false on success failure + */ + virtual bool enableCollision(const std::string& id, const bool enable); + + /** + * Get object pose. + * @param id string that identifies object in gazebo (returned after creation) + * @param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. + * @return returns value of the pose in the world reference frame + */ + virtual Pose getPose(const std::string& id, const std::string& frame_name = ""); + + /** + * Load a model from file. + * @param id string that specifies the name of the model + * @return returns true/false on success failure. + */ + virtual bool loadModelFromFile(const std::string& filename); + + /** + * Delete an object. + * @param id string that identifies object in gazebo (returned after creation) + * @return returns true/false on success failure. + */ + virtual bool deleteObject(const std::string& id); + + /** + * Delete all objects in the world. + */ + virtual bool deleteAll(); + + /** + * List id of all objects that have been added to the world. + * @return return a list of string containing the id of the objects + */ + virtual std::vector getList(); + + /** + * Attach an object to a link of the robot. + * @param id string that identifies object in gazebo (returned after creation) + * @param link_name name of a link of the robot + * @return true if success, false otherwise + */ + virtual bool attach(const std::string& id, const std::string& link_name); + + /** + * Detach a previously attached object. + * @param id string that identifies object in gazebo (returned after creation) + * @return true if success, false otherwise + */ + virtual bool detach(const std::string& id); + + /** + * Change the names of an object. + * @param old_name string that identifies object in gazebo + * @param new_name string that will be used as new name + * @return true if success, false otherwise + */ + virtual bool rename(const std::string& old_name, const std::string& new_name); + + // help method + virtual std::vector help(const std::string& functionName = "--all"); + + // read from ConnectionReader + bool read(yarp::os::ConnectionReader& connection) override; }; -#endif +} // namespace GazeboYarpPlugins + +#endif // YARP_THRIFT_GENERATOR_SERVICE_WORLDINTERFACESERVER_H diff --git a/thrift/worldinterface/autogenerated/src/Color.cpp b/thrift/worldinterface/autogenerated/src/Color.cpp index 40e5e88c0..3338a9735 100644 --- a/thrift/worldinterface/autogenerated/src/Color.cpp +++ b/thrift/worldinterface/autogenerated/src/Color.cpp @@ -1,222 +1,617 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. #include namespace GazeboYarpPlugins { -bool Color::read_r(yarp::os::idl::WireReader& reader) { - if (!reader.readI16(r)) { - reader.fail(); - return false; - } - return true; -} -bool Color::nested_read_r(yarp::os::idl::WireReader& reader) { - if (!reader.readI16(r)) { - reader.fail(); - return false; - } - return true; -} -bool Color::read_g(yarp::os::idl::WireReader& reader) { - if (!reader.readI16(g)) { - reader.fail(); - return false; - } - return true; -} -bool Color::nested_read_g(yarp::os::idl::WireReader& reader) { - if (!reader.readI16(g)) { - reader.fail(); - return false; - } - return true; -} -bool Color::read_b(yarp::os::idl::WireReader& reader) { - if (!reader.readI16(b)) { - reader.fail(); - return false; - } - return true; -} -bool Color::nested_read_b(yarp::os::idl::WireReader& reader) { - if (!reader.readI16(b)) { - reader.fail(); - return false; - } - return true; -} -bool Color::read(yarp::os::idl::WireReader& reader) { - if (!read_r(reader)) return false; - if (!read_g(reader)) return false; - if (!read_b(reader)) return false; - return !reader.isError(); -} - -bool Color::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListHeader(3)) return false; - return read(reader); -} - -bool Color::write_r(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeI16(r)) return false; - return true; -} -bool Color::nested_write_r(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeI16(r)) return false; - return true; -} -bool Color::write_g(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeI16(g)) return false; - return true; -} -bool Color::nested_write_g(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeI16(g)) return false; - return true; -} -bool Color::write_b(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeI16(b)) return false; - return true; -} -bool Color::nested_write_b(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeI16(b)) return false; - return true; -} -bool Color::write(const yarp::os::idl::WireWriter& writer) const { - if (!write_r(writer)) return false; - if (!write_g(writer)) return false; - if (!write_b(writer)) return false; - return !writer.isError(); -} - -bool Color::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(3)) return false; - return write(writer); -} -bool Color::Editor::write(yarp::os::ConnectionWriter& connection) const { - if (!isValid()) return false; - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(dirty_count+1)) return false; - if (!writer.writeString("patch")) return false; - if (is_dirty_r) { - if (!writer.writeListHeader(3)) return false; - if (!writer.writeString("set")) return false; - if (!writer.writeString("r")) return false; - if (!obj->nested_write_r(writer)) return false; - } - if (is_dirty_g) { - if (!writer.writeListHeader(3)) return false; - if (!writer.writeString("set")) return false; - if (!writer.writeString("g")) return false; - if (!obj->nested_write_g(writer)) return false; - } - if (is_dirty_b) { - if (!writer.writeListHeader(3)) return false; - if (!writer.writeString("set")) return false; - if (!writer.writeString("b")) return false; - if (!obj->nested_write_b(writer)) return false; - } - return !writer.isError(); -} -bool Color::Editor::read(yarp::os::ConnectionReader& connection) { - if (!isValid()) return false; - yarp::os::idl::WireReader reader(connection); - reader.expectAccept(); - if (!reader.readListHeader()) return false; - int len = reader.getLength(); - if (len==0) { - yarp::os::idl::WireWriter writer(reader); - if (writer.isNull()) return true; - if (!writer.writeListHeader(1)) return false; - writer.writeString("send: 'help' or 'patch (param1 val1) (param2 val2)'"); - return true; - } - std::string tag; - if (!reader.readString(tag)) return false; - if (tag=="help") { + +// Default constructor +Color::Color() : + WirePortable(), + r(0), + g(0), + b(0) +{ +} + +// Constructor with field values +Color::Color(const std::int16_t r, + const std::int16_t g, + const std::int16_t b) : + WirePortable(), + r(r), + g(g), + b(b) +{ +} + +// Read structure on a Wire +bool Color::read(yarp::os::idl::WireReader& reader) +{ + if (!read_r(reader)) { + return false; + } + if (!read_g(reader)) { + return false; + } + if (!read_b(reader)) { + return false; + } + return !reader.isError(); +} + +// Read structure on a Connection +bool Color::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListHeader(3)) { + return false; + } + return read(reader); +} + +// Write structure on a Wire +bool Color::write(const yarp::os::idl::WireWriter& writer) const +{ + if (!write_r(writer)) { + return false; + } + if (!write_g(writer)) { + return false; + } + if (!write_b(writer)) { + return false; + } + return !writer.isError(); +} + +// Write structure on a Connection +bool Color::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(3)) { + return false; + } + return write(writer); +} + +// Convert to a printable string +std::string Color::toString() const +{ + yarp::os::Bottle b; + b.read(*this); + return b.toString(); +} + +// Editor: default constructor +Color::Editor::Editor() +{ + group = 0; + obj_owned = true; + obj = new Color; + dirty_flags(false); + yarp().setOwner(*this); +} + +// Editor: constructor with base class +Color::Editor::Editor(Color& obj) +{ + group = 0; + obj_owned = false; + edit(obj, false); + yarp().setOwner(*this); +} + +// Editor: destructor +Color::Editor::~Editor() +{ + if (obj_owned) { + delete obj; + } +} + +// Editor: edit +bool Color::Editor::edit(Color& obj, bool dirty) +{ + if (obj_owned) { + delete this->obj; + } + this->obj = &obj; + obj_owned = false; + dirty_flags(dirty); + return true; +} + +// Editor: validity check +bool Color::Editor::isValid() const +{ + return obj != nullptr; +} + +// Editor: state +Color& Color::Editor::state() +{ + return *obj; +} + +// Editor: grouping begin +void Color::Editor::start_editing() +{ + group++; +} + +// Editor: grouping end +void Color::Editor::stop_editing() +{ + group--; + if (group == 0 && is_dirty) { + communicate(); + } +} +// Editor: r setter +void Color::Editor::set_r(const std::int16_t r) +{ + will_set_r(); + obj->r = r; + mark_dirty_r(); + communicate(); + did_set_r(); +} + +// Editor: r getter +std::int16_t Color::Editor::get_r() const +{ + return obj->r; +} + +// Editor: r will_set +bool Color::Editor::will_set_r() +{ + return true; +} + +// Editor: r did_set +bool Color::Editor::did_set_r() +{ + return true; +} + +// Editor: g setter +void Color::Editor::set_g(const std::int16_t g) +{ + will_set_g(); + obj->g = g; + mark_dirty_g(); + communicate(); + did_set_g(); +} + +// Editor: g getter +std::int16_t Color::Editor::get_g() const +{ + return obj->g; +} + +// Editor: g will_set +bool Color::Editor::will_set_g() +{ + return true; +} + +// Editor: g did_set +bool Color::Editor::did_set_g() +{ + return true; +} + +// Editor: b setter +void Color::Editor::set_b(const std::int16_t b) +{ + will_set_b(); + obj->b = b; + mark_dirty_b(); + communicate(); + did_set_b(); +} + +// Editor: b getter +std::int16_t Color::Editor::get_b() const +{ + return obj->b; +} + +// Editor: b will_set +bool Color::Editor::will_set_b() +{ + return true; +} + +// Editor: b did_set +bool Color::Editor::did_set_b() +{ + return true; +} + +// Editor: clean +void Color::Editor::clean() +{ + dirty_flags(false); +} + +// Editor: read +bool Color::Editor::read(yarp::os::ConnectionReader& connection) +{ + if (!isValid()) { + return false; + } + yarp::os::idl::WireReader reader(connection); + reader.expectAccept(); + if (!reader.readListHeader()) { + return false; + } + int len = reader.getLength(); + if (len == 0) { + yarp::os::idl::WireWriter writer(reader); + if (writer.isNull()) { + return true; + } + if (!writer.writeListHeader(1)) { + return false; + } + writer.writeString("send: 'help' or 'patch (param1 val1) (param2 val2)'"); + return true; + } + std::string tag; + if (!reader.readString(tag)) { + return false; + } + if (tag == "help") { + yarp::os::idl::WireWriter writer(reader); + if (writer.isNull()) { + return true; + } + if (!writer.writeListHeader(2)) { + return false; + } + if (!writer.writeTag("many", 1, 0)) { + return false; + } + if (reader.getLength() > 0) { + std::string field; + if (!reader.readString(field)) { + return false; + } + if (field == "r") { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString("std::int16_t r")) { + return false; + } + } + if (field == "g") { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString("std::int16_t g")) { + return false; + } + } + if (field == "b") { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString("std::int16_t b")) { + return false; + } + } + } + if (!writer.writeListHeader(4)) { + return false; + } + writer.writeString("*** Available fields:"); + writer.writeString("r"); + writer.writeString("g"); + writer.writeString("b"); + return true; + } + bool nested = true; + bool have_act = false; + if (tag != "patch") { + if (((len - 1) % 2) != 0) { + return false; + } + len = 1 + ((len - 1) / 2); + nested = false; + have_act = true; + } + for (int i = 1; i < len; ++i) { + if (nested && !reader.readListHeader(3)) { + return false; + } + std::string act; + std::string key; + if (have_act) { + act = tag; + } else if (!reader.readString(act)) { + return false; + } + if (!reader.readString(key)) { + return false; + } + if (key == "r") { + will_set_r(); + if (!obj->nested_read_r(reader)) { + return false; + } + did_set_r(); + } else if (key == "g") { + will_set_g(); + if (!obj->nested_read_g(reader)) { + return false; + } + did_set_g(); + } else if (key == "b") { + will_set_b(); + if (!obj->nested_read_b(reader)) { + return false; + } + did_set_b(); + } else { + // would be useful to have a fallback here + } + } + reader.accept(); yarp::os::idl::WireWriter writer(reader); - if (writer.isNull()) return true; - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("many",1, 0)) return false; - if (reader.getLength()>0) { - std::string field; - if (!reader.readString(field)) return false; - if (field=="r") { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString("std::int16_t r")) return false; - } - if (field=="g") { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString("std::int16_t g")) return false; - } - if (field=="b") { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString("std::int16_t b")) return false; - } - } - if (!writer.writeListHeader(4)) return false; - writer.writeString("*** Available fields:"); - writer.writeString("r"); - writer.writeString("g"); - writer.writeString("b"); - return true; - } - bool nested = true; - bool have_act = false; - if (tag!="patch") { - if ((len-1)%2 != 0) return false; - len = 1 + ((len-1)/2); - nested = false; - have_act = true; - } - for (int i=1; inested_read_r(reader)) return false; - did_set_r(); - } else if (key == "g") { - will_set_g(); - if (!obj->nested_read_g(reader)) return false; - did_set_g(); - } else if (key == "b") { - will_set_b(); - if (!obj->nested_read_b(reader)) return false; - did_set_b(); - } else { - // would be useful to have a fallback here - } - } - reader.accept(); - yarp::os::idl::WireWriter writer(reader); - if (writer.isNull()) return true; - writer.writeListHeader(1); - writer.writeVocab(yarp::os::createVocab('o','k')); - return true; -} - -std::string Color::toString() const { - yarp::os::Bottle b; - b.read(*this); - return b.toString(); -} -} // namespace + if (writer.isNull()) { + return true; + } + writer.writeListHeader(1); + writer.writeVocab(yarp::os::createVocab('o', 'k')); + return true; +} + +// Editor: write +bool Color::Editor::write(yarp::os::ConnectionWriter& connection) const +{ + if (!isValid()) { + return false; + } + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(dirty_count + 1)) { + return false; + } + if (!writer.writeString("patch")) { + return false; + } + if (is_dirty_r) { + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeString("set")) { + return false; + } + if (!writer.writeString("r")) { + return false; + } + if (!obj->nested_write_r(writer)) { + return false; + } + } + if (is_dirty_g) { + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeString("set")) { + return false; + } + if (!writer.writeString("g")) { + return false; + } + if (!obj->nested_write_g(writer)) { + return false; + } + } + if (is_dirty_b) { + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeString("set")) { + return false; + } + if (!writer.writeString("b")) { + return false; + } + if (!obj->nested_write_b(writer)) { + return false; + } + } + return !writer.isError(); +} + +// Editor: send if possible +void Color::Editor::communicate() +{ + if (group != 0) { + return; + } + if (yarp().canWrite()) { + yarp().write(*this); + clean(); + } +} + +// Editor: mark dirty overall +void Color::Editor::mark_dirty() +{ + is_dirty = true; +} + +// Editor: r mark_dirty +void Color::Editor::mark_dirty_r() +{ + if (is_dirty_r) { + return; + } + dirty_count++; + is_dirty_r = true; + mark_dirty(); +} + +// Editor: g mark_dirty +void Color::Editor::mark_dirty_g() +{ + if (is_dirty_g) { + return; + } + dirty_count++; + is_dirty_g = true; + mark_dirty(); +} + +// Editor: b mark_dirty +void Color::Editor::mark_dirty_b() +{ + if (is_dirty_b) { + return; + } + dirty_count++; + is_dirty_b = true; + mark_dirty(); +} + +// Editor: dirty_flags +void Color::Editor::dirty_flags(bool flag) +{ + is_dirty = flag; + is_dirty_r = flag; + is_dirty_g = flag; + is_dirty_b = flag; + dirty_count = flag ? 3 : 0; +} + +// read r field +bool Color::read_r(yarp::os::idl::WireReader& reader) +{ + if (!reader.readI16(r)) { + reader.fail(); + return false; + } + return true; +} + +// write r field +bool Color::write_r(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeI16(r)) { + return false; + } + return true; +} + +// read (nested) r field +bool Color::nested_read_r(yarp::os::idl::WireReader& reader) +{ + if (!reader.readI16(r)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) r field +bool Color::nested_write_r(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeI16(r)) { + return false; + } + return true; +} + +// read g field +bool Color::read_g(yarp::os::idl::WireReader& reader) +{ + if (!reader.readI16(g)) { + reader.fail(); + return false; + } + return true; +} + +// write g field +bool Color::write_g(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeI16(g)) { + return false; + } + return true; +} + +// read (nested) g field +bool Color::nested_read_g(yarp::os::idl::WireReader& reader) +{ + if (!reader.readI16(g)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) g field +bool Color::nested_write_g(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeI16(g)) { + return false; + } + return true; +} + +// read b field +bool Color::read_b(yarp::os::idl::WireReader& reader) +{ + if (!reader.readI16(b)) { + reader.fail(); + return false; + } + return true; +} + +// write b field +bool Color::write_b(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeI16(b)) { + return false; + } + return true; +} + +// read (nested) b field +bool Color::nested_read_b(yarp::os::idl::WireReader& reader) +{ + if (!reader.readI16(b)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) b field +bool Color::nested_write_b(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeI16(b)) { + return false; + } + return true; +} + +} // namespace GazeboYarpPlugins diff --git a/thrift/worldinterface/autogenerated/src/Pose.cpp b/thrift/worldinterface/autogenerated/src/Pose.cpp index 17f6ba98d..6a34f5c4b 100644 --- a/thrift/worldinterface/autogenerated/src/Pose.cpp +++ b/thrift/worldinterface/autogenerated/src/Pose.cpp @@ -1,339 +1,965 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. #include namespace GazeboYarpPlugins { -bool Pose::read_x(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(x)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::nested_read_x(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(x)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::read_y(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(y)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::nested_read_y(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(y)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::read_z(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(z)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::nested_read_z(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(z)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::read_roll(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(roll)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::nested_read_roll(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(roll)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::read_pitch(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(pitch)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::nested_read_pitch(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(pitch)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::read_yaw(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(yaw)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::nested_read_yaw(yarp::os::idl::WireReader& reader) { - if (!reader.readFloat64(yaw)) { - reader.fail(); - return false; - } - return true; -} -bool Pose::read(yarp::os::idl::WireReader& reader) { - if (!read_x(reader)) return false; - if (!read_y(reader)) return false; - if (!read_z(reader)) return false; - if (!read_roll(reader)) return false; - if (!read_pitch(reader)) return false; - if (!read_yaw(reader)) return false; - return !reader.isError(); -} - -bool Pose::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListHeader(6)) return false; - return read(reader); -} - -bool Pose::write_x(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(x)) return false; - return true; -} -bool Pose::nested_write_x(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(x)) return false; - return true; -} -bool Pose::write_y(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(y)) return false; - return true; -} -bool Pose::nested_write_y(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(y)) return false; - return true; -} -bool Pose::write_z(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(z)) return false; - return true; -} -bool Pose::nested_write_z(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(z)) return false; - return true; -} -bool Pose::write_roll(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(roll)) return false; - return true; -} -bool Pose::nested_write_roll(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(roll)) return false; - return true; -} -bool Pose::write_pitch(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(pitch)) return false; - return true; -} -bool Pose::nested_write_pitch(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(pitch)) return false; - return true; -} -bool Pose::write_yaw(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(yaw)) return false; - return true; -} -bool Pose::nested_write_yaw(const yarp::os::idl::WireWriter& writer) const { - if (!writer.writeFloat64(yaw)) return false; - return true; -} -bool Pose::write(const yarp::os::idl::WireWriter& writer) const { - if (!write_x(writer)) return false; - if (!write_y(writer)) return false; - if (!write_z(writer)) return false; - if (!write_roll(writer)) return false; - if (!write_pitch(writer)) return false; - if (!write_yaw(writer)) return false; - return !writer.isError(); -} - -bool Pose::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(6)) return false; - return write(writer); -} -bool Pose::Editor::write(yarp::os::ConnectionWriter& connection) const { - if (!isValid()) return false; - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(dirty_count+1)) return false; - if (!writer.writeString("patch")) return false; - if (is_dirty_x) { - if (!writer.writeListHeader(3)) return false; - if (!writer.writeString("set")) return false; - if (!writer.writeString("x")) return false; - if (!obj->nested_write_x(writer)) return false; - } - if (is_dirty_y) { - if (!writer.writeListHeader(3)) return false; - if (!writer.writeString("set")) return false; - if (!writer.writeString("y")) return false; - if (!obj->nested_write_y(writer)) return false; - } - if (is_dirty_z) { - if (!writer.writeListHeader(3)) return false; - if (!writer.writeString("set")) return false; - if (!writer.writeString("z")) return false; - if (!obj->nested_write_z(writer)) return false; - } - if (is_dirty_roll) { - if (!writer.writeListHeader(3)) return false; - if (!writer.writeString("set")) return false; - if (!writer.writeString("roll")) return false; - if (!obj->nested_write_roll(writer)) return false; - } - if (is_dirty_pitch) { - if (!writer.writeListHeader(3)) return false; - if (!writer.writeString("set")) return false; - if (!writer.writeString("pitch")) return false; - if (!obj->nested_write_pitch(writer)) return false; - } - if (is_dirty_yaw) { - if (!writer.writeListHeader(3)) return false; - if (!writer.writeString("set")) return false; - if (!writer.writeString("yaw")) return false; - if (!obj->nested_write_yaw(writer)) return false; - } - return !writer.isError(); -} -bool Pose::Editor::read(yarp::os::ConnectionReader& connection) { - if (!isValid()) return false; - yarp::os::idl::WireReader reader(connection); - reader.expectAccept(); - if (!reader.readListHeader()) return false; - int len = reader.getLength(); - if (len==0) { - yarp::os::idl::WireWriter writer(reader); - if (writer.isNull()) return true; - if (!writer.writeListHeader(1)) return false; - writer.writeString("send: 'help' or 'patch (param1 val1) (param2 val2)'"); - return true; - } - std::string tag; - if (!reader.readString(tag)) return false; - if (tag=="help") { + +// Default constructor +Pose::Pose() : + WirePortable(), + x(0), + y(0), + z(0), + roll(0), + pitch(0), + yaw(0) +{ +} + +// Constructor with field values +Pose::Pose(const double x, + const double y, + const double z, + const double roll, + const double pitch, + const double yaw) : + WirePortable(), + x(x), + y(y), + z(z), + roll(roll), + pitch(pitch), + yaw(yaw) +{ +} + +// Read structure on a Wire +bool Pose::read(yarp::os::idl::WireReader& reader) +{ + if (!read_x(reader)) { + return false; + } + if (!read_y(reader)) { + return false; + } + if (!read_z(reader)) { + return false; + } + if (!read_roll(reader)) { + return false; + } + if (!read_pitch(reader)) { + return false; + } + if (!read_yaw(reader)) { + return false; + } + return !reader.isError(); +} + +// Read structure on a Connection +bool Pose::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListHeader(6)) { + return false; + } + return read(reader); +} + +// Write structure on a Wire +bool Pose::write(const yarp::os::idl::WireWriter& writer) const +{ + if (!write_x(writer)) { + return false; + } + if (!write_y(writer)) { + return false; + } + if (!write_z(writer)) { + return false; + } + if (!write_roll(writer)) { + return false; + } + if (!write_pitch(writer)) { + return false; + } + if (!write_yaw(writer)) { + return false; + } + return !writer.isError(); +} + +// Write structure on a Connection +bool Pose::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(6)) { + return false; + } + return write(writer); +} + +// Convert to a printable string +std::string Pose::toString() const +{ + yarp::os::Bottle b; + b.read(*this); + return b.toString(); +} + +// Editor: default constructor +Pose::Editor::Editor() +{ + group = 0; + obj_owned = true; + obj = new Pose; + dirty_flags(false); + yarp().setOwner(*this); +} + +// Editor: constructor with base class +Pose::Editor::Editor(Pose& obj) +{ + group = 0; + obj_owned = false; + edit(obj, false); + yarp().setOwner(*this); +} + +// Editor: destructor +Pose::Editor::~Editor() +{ + if (obj_owned) { + delete obj; + } +} + +// Editor: edit +bool Pose::Editor::edit(Pose& obj, bool dirty) +{ + if (obj_owned) { + delete this->obj; + } + this->obj = &obj; + obj_owned = false; + dirty_flags(dirty); + return true; +} + +// Editor: validity check +bool Pose::Editor::isValid() const +{ + return obj != nullptr; +} + +// Editor: state +Pose& Pose::Editor::state() +{ + return *obj; +} + +// Editor: grouping begin +void Pose::Editor::start_editing() +{ + group++; +} + +// Editor: grouping end +void Pose::Editor::stop_editing() +{ + group--; + if (group == 0 && is_dirty) { + communicate(); + } +} +// Editor: x setter +void Pose::Editor::set_x(const double x) +{ + will_set_x(); + obj->x = x; + mark_dirty_x(); + communicate(); + did_set_x(); +} + +// Editor: x getter +double Pose::Editor::get_x() const +{ + return obj->x; +} + +// Editor: x will_set +bool Pose::Editor::will_set_x() +{ + return true; +} + +// Editor: x did_set +bool Pose::Editor::did_set_x() +{ + return true; +} + +// Editor: y setter +void Pose::Editor::set_y(const double y) +{ + will_set_y(); + obj->y = y; + mark_dirty_y(); + communicate(); + did_set_y(); +} + +// Editor: y getter +double Pose::Editor::get_y() const +{ + return obj->y; +} + +// Editor: y will_set +bool Pose::Editor::will_set_y() +{ + return true; +} + +// Editor: y did_set +bool Pose::Editor::did_set_y() +{ + return true; +} + +// Editor: z setter +void Pose::Editor::set_z(const double z) +{ + will_set_z(); + obj->z = z; + mark_dirty_z(); + communicate(); + did_set_z(); +} + +// Editor: z getter +double Pose::Editor::get_z() const +{ + return obj->z; +} + +// Editor: z will_set +bool Pose::Editor::will_set_z() +{ + return true; +} + +// Editor: z did_set +bool Pose::Editor::did_set_z() +{ + return true; +} + +// Editor: roll setter +void Pose::Editor::set_roll(const double roll) +{ + will_set_roll(); + obj->roll = roll; + mark_dirty_roll(); + communicate(); + did_set_roll(); +} + +// Editor: roll getter +double Pose::Editor::get_roll() const +{ + return obj->roll; +} + +// Editor: roll will_set +bool Pose::Editor::will_set_roll() +{ + return true; +} + +// Editor: roll did_set +bool Pose::Editor::did_set_roll() +{ + return true; +} + +// Editor: pitch setter +void Pose::Editor::set_pitch(const double pitch) +{ + will_set_pitch(); + obj->pitch = pitch; + mark_dirty_pitch(); + communicate(); + did_set_pitch(); +} + +// Editor: pitch getter +double Pose::Editor::get_pitch() const +{ + return obj->pitch; +} + +// Editor: pitch will_set +bool Pose::Editor::will_set_pitch() +{ + return true; +} + +// Editor: pitch did_set +bool Pose::Editor::did_set_pitch() +{ + return true; +} + +// Editor: yaw setter +void Pose::Editor::set_yaw(const double yaw) +{ + will_set_yaw(); + obj->yaw = yaw; + mark_dirty_yaw(); + communicate(); + did_set_yaw(); +} + +// Editor: yaw getter +double Pose::Editor::get_yaw() const +{ + return obj->yaw; +} + +// Editor: yaw will_set +bool Pose::Editor::will_set_yaw() +{ + return true; +} + +// Editor: yaw did_set +bool Pose::Editor::did_set_yaw() +{ + return true; +} + +// Editor: clean +void Pose::Editor::clean() +{ + dirty_flags(false); +} + +// Editor: read +bool Pose::Editor::read(yarp::os::ConnectionReader& connection) +{ + if (!isValid()) { + return false; + } + yarp::os::idl::WireReader reader(connection); + reader.expectAccept(); + if (!reader.readListHeader()) { + return false; + } + int len = reader.getLength(); + if (len == 0) { + yarp::os::idl::WireWriter writer(reader); + if (writer.isNull()) { + return true; + } + if (!writer.writeListHeader(1)) { + return false; + } + writer.writeString("send: 'help' or 'patch (param1 val1) (param2 val2)'"); + return true; + } + std::string tag; + if (!reader.readString(tag)) { + return false; + } + if (tag == "help") { + yarp::os::idl::WireWriter writer(reader); + if (writer.isNull()) { + return true; + } + if (!writer.writeListHeader(2)) { + return false; + } + if (!writer.writeTag("many", 1, 0)) { + return false; + } + if (reader.getLength() > 0) { + std::string field; + if (!reader.readString(field)) { + return false; + } + if (field == "x") { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString("double x")) { + return false; + } + } + if (field == "y") { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString("double y")) { + return false; + } + } + if (field == "z") { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString("double z")) { + return false; + } + } + if (field == "roll") { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString("double roll")) { + return false; + } + } + if (field == "pitch") { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString("double pitch")) { + return false; + } + } + if (field == "yaw") { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString("double yaw")) { + return false; + } + } + } + if (!writer.writeListHeader(7)) { + return false; + } + writer.writeString("*** Available fields:"); + writer.writeString("x"); + writer.writeString("y"); + writer.writeString("z"); + writer.writeString("roll"); + writer.writeString("pitch"); + writer.writeString("yaw"); + return true; + } + bool nested = true; + bool have_act = false; + if (tag != "patch") { + if (((len - 1) % 2) != 0) { + return false; + } + len = 1 + ((len - 1) / 2); + nested = false; + have_act = true; + } + for (int i = 1; i < len; ++i) { + if (nested && !reader.readListHeader(3)) { + return false; + } + std::string act; + std::string key; + if (have_act) { + act = tag; + } else if (!reader.readString(act)) { + return false; + } + if (!reader.readString(key)) { + return false; + } + if (key == "x") { + will_set_x(); + if (!obj->nested_read_x(reader)) { + return false; + } + did_set_x(); + } else if (key == "y") { + will_set_y(); + if (!obj->nested_read_y(reader)) { + return false; + } + did_set_y(); + } else if (key == "z") { + will_set_z(); + if (!obj->nested_read_z(reader)) { + return false; + } + did_set_z(); + } else if (key == "roll") { + will_set_roll(); + if (!obj->nested_read_roll(reader)) { + return false; + } + did_set_roll(); + } else if (key == "pitch") { + will_set_pitch(); + if (!obj->nested_read_pitch(reader)) { + return false; + } + did_set_pitch(); + } else if (key == "yaw") { + will_set_yaw(); + if (!obj->nested_read_yaw(reader)) { + return false; + } + did_set_yaw(); + } else { + // would be useful to have a fallback here + } + } + reader.accept(); yarp::os::idl::WireWriter writer(reader); - if (writer.isNull()) return true; - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("many",1, 0)) return false; - if (reader.getLength()>0) { - std::string field; - if (!reader.readString(field)) return false; - if (field=="x") { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString("double x")) return false; - } - if (field=="y") { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString("double y")) return false; - } - if (field=="z") { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString("double z")) return false; - } - if (field=="roll") { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString("double roll")) return false; - } - if (field=="pitch") { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString("double pitch")) return false; - } - if (field=="yaw") { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString("double yaw")) return false; - } - } - if (!writer.writeListHeader(7)) return false; - writer.writeString("*** Available fields:"); - writer.writeString("x"); - writer.writeString("y"); - writer.writeString("z"); - writer.writeString("roll"); - writer.writeString("pitch"); - writer.writeString("yaw"); - return true; - } - bool nested = true; - bool have_act = false; - if (tag!="patch") { - if ((len-1)%2 != 0) return false; - len = 1 + ((len-1)/2); - nested = false; - have_act = true; - } - for (int i=1; inested_read_x(reader)) return false; - did_set_x(); - } else if (key == "y") { - will_set_y(); - if (!obj->nested_read_y(reader)) return false; - did_set_y(); - } else if (key == "z") { - will_set_z(); - if (!obj->nested_read_z(reader)) return false; - did_set_z(); - } else if (key == "roll") { - will_set_roll(); - if (!obj->nested_read_roll(reader)) return false; - did_set_roll(); - } else if (key == "pitch") { - will_set_pitch(); - if (!obj->nested_read_pitch(reader)) return false; - did_set_pitch(); - } else if (key == "yaw") { - will_set_yaw(); - if (!obj->nested_read_yaw(reader)) return false; - did_set_yaw(); - } else { - // would be useful to have a fallback here - } - } - reader.accept(); - yarp::os::idl::WireWriter writer(reader); - if (writer.isNull()) return true; - writer.writeListHeader(1); - writer.writeVocab(yarp::os::createVocab('o','k')); - return true; -} - -std::string Pose::toString() const { - yarp::os::Bottle b; - b.read(*this); - return b.toString(); -} -} // namespace + if (writer.isNull()) { + return true; + } + writer.writeListHeader(1); + writer.writeVocab(yarp::os::createVocab('o', 'k')); + return true; +} + +// Editor: write +bool Pose::Editor::write(yarp::os::ConnectionWriter& connection) const +{ + if (!isValid()) { + return false; + } + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(dirty_count + 1)) { + return false; + } + if (!writer.writeString("patch")) { + return false; + } + if (is_dirty_x) { + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeString("set")) { + return false; + } + if (!writer.writeString("x")) { + return false; + } + if (!obj->nested_write_x(writer)) { + return false; + } + } + if (is_dirty_y) { + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeString("set")) { + return false; + } + if (!writer.writeString("y")) { + return false; + } + if (!obj->nested_write_y(writer)) { + return false; + } + } + if (is_dirty_z) { + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeString("set")) { + return false; + } + if (!writer.writeString("z")) { + return false; + } + if (!obj->nested_write_z(writer)) { + return false; + } + } + if (is_dirty_roll) { + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeString("set")) { + return false; + } + if (!writer.writeString("roll")) { + return false; + } + if (!obj->nested_write_roll(writer)) { + return false; + } + } + if (is_dirty_pitch) { + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeString("set")) { + return false; + } + if (!writer.writeString("pitch")) { + return false; + } + if (!obj->nested_write_pitch(writer)) { + return false; + } + } + if (is_dirty_yaw) { + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeString("set")) { + return false; + } + if (!writer.writeString("yaw")) { + return false; + } + if (!obj->nested_write_yaw(writer)) { + return false; + } + } + return !writer.isError(); +} + +// Editor: send if possible +void Pose::Editor::communicate() +{ + if (group != 0) { + return; + } + if (yarp().canWrite()) { + yarp().write(*this); + clean(); + } +} + +// Editor: mark dirty overall +void Pose::Editor::mark_dirty() +{ + is_dirty = true; +} + +// Editor: x mark_dirty +void Pose::Editor::mark_dirty_x() +{ + if (is_dirty_x) { + return; + } + dirty_count++; + is_dirty_x = true; + mark_dirty(); +} + +// Editor: y mark_dirty +void Pose::Editor::mark_dirty_y() +{ + if (is_dirty_y) { + return; + } + dirty_count++; + is_dirty_y = true; + mark_dirty(); +} + +// Editor: z mark_dirty +void Pose::Editor::mark_dirty_z() +{ + if (is_dirty_z) { + return; + } + dirty_count++; + is_dirty_z = true; + mark_dirty(); +} + +// Editor: roll mark_dirty +void Pose::Editor::mark_dirty_roll() +{ + if (is_dirty_roll) { + return; + } + dirty_count++; + is_dirty_roll = true; + mark_dirty(); +} + +// Editor: pitch mark_dirty +void Pose::Editor::mark_dirty_pitch() +{ + if (is_dirty_pitch) { + return; + } + dirty_count++; + is_dirty_pitch = true; + mark_dirty(); +} + +// Editor: yaw mark_dirty +void Pose::Editor::mark_dirty_yaw() +{ + if (is_dirty_yaw) { + return; + } + dirty_count++; + is_dirty_yaw = true; + mark_dirty(); +} + +// Editor: dirty_flags +void Pose::Editor::dirty_flags(bool flag) +{ + is_dirty = flag; + is_dirty_x = flag; + is_dirty_y = flag; + is_dirty_z = flag; + is_dirty_roll = flag; + is_dirty_pitch = flag; + is_dirty_yaw = flag; + dirty_count = flag ? 6 : 0; +} + +// read x field +bool Pose::read_x(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(x)) { + reader.fail(); + return false; + } + return true; +} + +// write x field +bool Pose::write_x(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(x)) { + return false; + } + return true; +} + +// read (nested) x field +bool Pose::nested_read_x(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(x)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) x field +bool Pose::nested_write_x(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(x)) { + return false; + } + return true; +} + +// read y field +bool Pose::read_y(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(y)) { + reader.fail(); + return false; + } + return true; +} + +// write y field +bool Pose::write_y(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(y)) { + return false; + } + return true; +} + +// read (nested) y field +bool Pose::nested_read_y(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(y)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) y field +bool Pose::nested_write_y(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(y)) { + return false; + } + return true; +} + +// read z field +bool Pose::read_z(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(z)) { + reader.fail(); + return false; + } + return true; +} + +// write z field +bool Pose::write_z(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(z)) { + return false; + } + return true; +} + +// read (nested) z field +bool Pose::nested_read_z(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(z)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) z field +bool Pose::nested_write_z(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(z)) { + return false; + } + return true; +} + +// read roll field +bool Pose::read_roll(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(roll)) { + reader.fail(); + return false; + } + return true; +} + +// write roll field +bool Pose::write_roll(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(roll)) { + return false; + } + return true; +} + +// read (nested) roll field +bool Pose::nested_read_roll(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(roll)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) roll field +bool Pose::nested_write_roll(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(roll)) { + return false; + } + return true; +} + +// read pitch field +bool Pose::read_pitch(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(pitch)) { + reader.fail(); + return false; + } + return true; +} + +// write pitch field +bool Pose::write_pitch(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(pitch)) { + return false; + } + return true; +} + +// read (nested) pitch field +bool Pose::nested_read_pitch(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(pitch)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) pitch field +bool Pose::nested_write_pitch(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(pitch)) { + return false; + } + return true; +} + +// read yaw field +bool Pose::read_yaw(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(yaw)) { + reader.fail(); + return false; + } + return true; +} + +// write yaw field +bool Pose::write_yaw(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(yaw)) { + return false; + } + return true; +} + +// read (nested) yaw field +bool Pose::nested_read_yaw(yarp::os::idl::WireReader& reader) +{ + if (!reader.readFloat64(yaw)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) yaw field +bool Pose::nested_write_yaw(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeFloat64(yaw)) { + return false; + } + return true; +} + +} // namespace GazeboYarpPlugins diff --git a/thrift/worldinterface/autogenerated/src/WorldInterfaceServer.cpp b/thrift/worldinterface/autogenerated/src/WorldInterfaceServer.cpp index e2dc21efe..25ede17b8 100644 --- a/thrift/worldinterface/autogenerated/src/WorldInterfaceServer.cpp +++ b/thrift/worldinterface/autogenerated/src/WorldInterfaceServer.cpp @@ -1,1416 +1,1808 @@ /* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * * This software may be modified and distributed under the terms of the * BSD-3-Clause license. See the accompanying LICENSE file for details. */ +// Autogenerated by Thrift Compiler (0.12.0-yarped) +// // This is an automatically generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. #include + #include namespace GazeboYarpPlugins { - -class WorldInterfaceServer_makeSphere : public yarp::os::Portable { -public: - double radius; - Pose pose; - Color color; - std::string frame_name; - std::string object_name; - bool gravity_enable; - bool collision_enable; - std::string _return; - void init(const double radius, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; - -class WorldInterfaceServer_makeBox : public yarp::os::Portable { +class WorldInterfaceServer_makeSphere : + public yarp::os::Portable +{ public: - double width; - double height; - double thickness; - Pose pose; - Color color; - std::string frame_name; - std::string object_name; - bool gravity_enable; - bool collision_enable; - std::string _return; - void init(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + double radius; + Pose pose; + Color color; + std::string frame_name; + std::string object_name; + bool gravity_enable; + bool collision_enable; + std::string _return; + void init(const double radius, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class WorldInterfaceServer_makeCylinder : public yarp::os::Portable { -public: - double radius; - double length; - Pose pose; - Color color; - std::string frame_name; - std::string object_name; - bool gravity_enable; - bool collision_enable; - std::string _return; - void init(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +bool WorldInterfaceServer_makeSphere::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(15)) { + return false; + } + if (!writer.writeTag("makeSphere", 1, 1)) { + return false; + } + if (!writer.writeFloat64(radius)) { + return false; + } + if (!writer.write(pose)) { + return false; + } + if (!writer.write(color)) { + return false; + } + if (!writer.writeString(frame_name)) { + return false; + } + if (!writer.writeString(object_name)) { + return false; + } + if (!writer.writeBool(gravity_enable)) { + return false; + } + if (!writer.writeBool(collision_enable)) { + return false; + } + return true; +} -class WorldInterfaceServer_makeFrame : public yarp::os::Portable { -public: - double size; - Pose pose; - Color color; - std::string frame_name; - std::string object_name; - bool gravity_enable; - bool collision_enable; - std::string _return; - void init(const double size, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +bool WorldInterfaceServer_makeSphere::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readString(_return)) { + reader.fail(); + return false; + } + return true; +} -class WorldInterfaceServer_changeColor : public yarp::os::Portable { -public: - std::string id; - Color color; - bool _return; - void init(const std::string& id, const Color& color); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +void WorldInterfaceServer_makeSphere::init(const double radius, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) +{ + _return = ""; + this->radius = radius; + this->pose = pose; + this->color = color; + this->frame_name = frame_name; + this->object_name = object_name; + this->gravity_enable = gravity_enable; + this->collision_enable = collision_enable; +} -class WorldInterfaceServer_setPose : public yarp::os::Portable { +class WorldInterfaceServer_makeBox : + public yarp::os::Portable +{ public: - std::string id; - Pose pose; - std::string frame_name; - bool _return; - void init(const std::string& id, const Pose& pose, const std::string& frame_name); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + double width; + double height; + double thickness; + Pose pose; + Color color; + std::string frame_name; + std::string object_name; + bool gravity_enable; + bool collision_enable; + std::string _return; + void init(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class WorldInterfaceServer_enableGravity : public yarp::os::Portable { -public: - std::string id; - bool enable; - bool _return; - void init(const std::string& id, const bool enable); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +bool WorldInterfaceServer_makeBox::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(17)) { + return false; + } + if (!writer.writeTag("makeBox", 1, 1)) { + return false; + } + if (!writer.writeFloat64(width)) { + return false; + } + if (!writer.writeFloat64(height)) { + return false; + } + if (!writer.writeFloat64(thickness)) { + return false; + } + if (!writer.write(pose)) { + return false; + } + if (!writer.write(color)) { + return false; + } + if (!writer.writeString(frame_name)) { + return false; + } + if (!writer.writeString(object_name)) { + return false; + } + if (!writer.writeBool(gravity_enable)) { + return false; + } + if (!writer.writeBool(collision_enable)) { + return false; + } + return true; +} -class WorldInterfaceServer_enableCollision : public yarp::os::Portable { +bool WorldInterfaceServer_makeBox::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readString(_return)) { + reader.fail(); + return false; + } + return true; +} + +void WorldInterfaceServer_makeBox::init(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) +{ + _return = ""; + this->width = width; + this->height = height; + this->thickness = thickness; + this->pose = pose; + this->color = color; + this->frame_name = frame_name; + this->object_name = object_name; + this->gravity_enable = gravity_enable; + this->collision_enable = collision_enable; +} + +class WorldInterfaceServer_makeCylinder : + public yarp::os::Portable +{ public: - std::string id; - bool enable; - bool _return; - void init(const std::string& id, const bool enable); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + double radius; + double length; + Pose pose; + Color color; + std::string frame_name; + std::string object_name; + bool gravity_enable; + bool collision_enable; + std::string _return; + void init(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class WorldInterfaceServer_getPose : public yarp::os::Portable { -public: - std::string id; - std::string frame_name; - Pose _return; - void init(const std::string& id, const std::string& frame_name); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +bool WorldInterfaceServer_makeCylinder::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(16)) { + return false; + } + if (!writer.writeTag("makeCylinder", 1, 1)) { + return false; + } + if (!writer.writeFloat64(radius)) { + return false; + } + if (!writer.writeFloat64(length)) { + return false; + } + if (!writer.write(pose)) { + return false; + } + if (!writer.write(color)) { + return false; + } + if (!writer.writeString(frame_name)) { + return false; + } + if (!writer.writeString(object_name)) { + return false; + } + if (!writer.writeBool(gravity_enable)) { + return false; + } + if (!writer.writeBool(collision_enable)) { + return false; + } + return true; +} -class WorldInterfaceServer_loadModelFromFile : public yarp::os::Portable { -public: - std::string filename; - bool _return; - void init(const std::string& filename); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +bool WorldInterfaceServer_makeCylinder::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readString(_return)) { + reader.fail(); + return false; + } + return true; +} -class WorldInterfaceServer_deleteObject : public yarp::os::Portable { -public: - std::string id; - bool _return; - void init(const std::string& id); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +void WorldInterfaceServer_makeCylinder::init(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) +{ + _return = ""; + this->radius = radius; + this->length = length; + this->pose = pose; + this->color = color; + this->frame_name = frame_name; + this->object_name = object_name; + this->gravity_enable = gravity_enable; + this->collision_enable = collision_enable; +} -class WorldInterfaceServer_deleteAll : public yarp::os::Portable { +class WorldInterfaceServer_makeFrame : + public yarp::os::Portable +{ public: - bool _return; - void init(); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + double size; + Pose pose; + Color color; + std::string frame_name; + std::string object_name; + bool gravity_enable; + bool collision_enable; + std::string _return; + void init(const double size, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -class WorldInterfaceServer_getList : public yarp::os::Portable { -public: - std::vector _return; - void init(); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +bool WorldInterfaceServer_makeFrame::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(15)) { + return false; + } + if (!writer.writeTag("makeFrame", 1, 1)) { + return false; + } + if (!writer.writeFloat64(size)) { + return false; + } + if (!writer.write(pose)) { + return false; + } + if (!writer.write(color)) { + return false; + } + if (!writer.writeString(frame_name)) { + return false; + } + if (!writer.writeString(object_name)) { + return false; + } + if (!writer.writeBool(gravity_enable)) { + return false; + } + if (!writer.writeBool(collision_enable)) { + return false; + } + return true; +} -class WorldInterfaceServer_attach : public yarp::os::Portable { -public: - std::string id; - std::string link_name; - bool _return; - void init(const std::string& id, const std::string& link_name); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +bool WorldInterfaceServer_makeFrame::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readString(_return)) { + reader.fail(); + return false; + } + return true; +} -class WorldInterfaceServer_detach : public yarp::os::Portable { -public: - std::string id; - bool _return; - void init(const std::string& id); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; -}; +void WorldInterfaceServer_makeFrame::init(const double size, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) +{ + _return = ""; + this->size = size; + this->pose = pose; + this->color = color; + this->frame_name = frame_name; + this->object_name = object_name; + this->gravity_enable = gravity_enable; + this->collision_enable = collision_enable; +} -class WorldInterfaceServer_rename : public yarp::os::Portable { +class WorldInterfaceServer_changeColor : + public yarp::os::Portable +{ public: - std::string old_name; - std::string new_name; - bool _return; - void init(const std::string& old_name, const std::string& new_name); - virtual bool write(yarp::os::ConnectionWriter& connection) const override; - virtual bool read(yarp::os::ConnectionReader& connection) override; + std::string id; + Color color; + bool _return; + void init(const std::string& id, const Color& color); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; }; -bool WorldInterfaceServer_makeSphere::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(15)) return false; - if (!writer.writeTag("makeSphere",1,1)) return false; - if (!writer.writeFloat64(radius)) return false; - if (!writer.write(pose)) return false; - if (!writer.write(color)) return false; - if (!writer.writeString(frame_name)) return false; - if (!writer.writeString(object_name)) return false; - if (!writer.writeBool(gravity_enable)) return false; - if (!writer.writeBool(collision_enable)) return false; - return true; -} - -bool WorldInterfaceServer_makeSphere::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readString(_return)) { - reader.fail(); - return false; - } - return true; -} - -void WorldInterfaceServer_makeSphere::init(const double radius, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) { - _return = ""; - this->radius = radius; - this->pose = pose; - this->color = color; - this->frame_name = frame_name; - this->object_name = object_name; - this->gravity_enable = gravity_enable; - this->collision_enable = collision_enable; -} - -bool WorldInterfaceServer_makeBox::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(17)) return false; - if (!writer.writeTag("makeBox",1,1)) return false; - if (!writer.writeFloat64(width)) return false; - if (!writer.writeFloat64(height)) return false; - if (!writer.writeFloat64(thickness)) return false; - if (!writer.write(pose)) return false; - if (!writer.write(color)) return false; - if (!writer.writeString(frame_name)) return false; - if (!writer.writeString(object_name)) return false; - if (!writer.writeBool(gravity_enable)) return false; - if (!writer.writeBool(collision_enable)) return false; - return true; -} - -bool WorldInterfaceServer_makeBox::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readString(_return)) { - reader.fail(); - return false; - } - return true; -} - -void WorldInterfaceServer_makeBox::init(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) { - _return = ""; - this->width = width; - this->height = height; - this->thickness = thickness; - this->pose = pose; - this->color = color; - this->frame_name = frame_name; - this->object_name = object_name; - this->gravity_enable = gravity_enable; - this->collision_enable = collision_enable; -} - -bool WorldInterfaceServer_makeCylinder::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(16)) return false; - if (!writer.writeTag("makeCylinder",1,1)) return false; - if (!writer.writeFloat64(radius)) return false; - if (!writer.writeFloat64(length)) return false; - if (!writer.write(pose)) return false; - if (!writer.write(color)) return false; - if (!writer.writeString(frame_name)) return false; - if (!writer.writeString(object_name)) return false; - if (!writer.writeBool(gravity_enable)) return false; - if (!writer.writeBool(collision_enable)) return false; - return true; -} - -bool WorldInterfaceServer_makeCylinder::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readString(_return)) { - reader.fail(); - return false; - } - return true; -} - -void WorldInterfaceServer_makeCylinder::init(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) { - _return = ""; - this->radius = radius; - this->length = length; - this->pose = pose; - this->color = color; - this->frame_name = frame_name; - this->object_name = object_name; - this->gravity_enable = gravity_enable; - this->collision_enable = collision_enable; -} - -bool WorldInterfaceServer_makeFrame::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(15)) return false; - if (!writer.writeTag("makeFrame",1,1)) return false; - if (!writer.writeFloat64(size)) return false; - if (!writer.write(pose)) return false; - if (!writer.write(color)) return false; - if (!writer.writeString(frame_name)) return false; - if (!writer.writeString(object_name)) return false; - if (!writer.writeBool(gravity_enable)) return false; - if (!writer.writeBool(collision_enable)) return false; - return true; -} - -bool WorldInterfaceServer_makeFrame::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readString(_return)) { - reader.fail(); - return false; - } - return true; -} - -void WorldInterfaceServer_makeFrame::init(const double size, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) { - _return = ""; - this->size = size; - this->pose = pose; - this->color = color; - this->frame_name = frame_name; - this->object_name = object_name; - this->gravity_enable = gravity_enable; - this->collision_enable = collision_enable; -} - -bool WorldInterfaceServer_changeColor::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(5)) return false; - if (!writer.writeTag("changeColor",1,1)) return false; - if (!writer.writeString(id)) return false; - if (!writer.write(color)) return false; - return true; -} - -bool WorldInterfaceServer_changeColor::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; +bool WorldInterfaceServer_changeColor::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(5)) { + return false; + } + if (!writer.writeTag("changeColor", 1, 1)) { + return false; + } + if (!writer.writeString(id)) { + return false; + } + if (!writer.write(color)) { + return false; + } + return true; } -void WorldInterfaceServer_changeColor::init(const std::string& id, const Color& color) { - _return = false; - this->id = id; - this->color = color; +bool WorldInterfaceServer_changeColor::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readBool(_return)) { + reader.fail(); + return false; + } + return true; } -bool WorldInterfaceServer_setPose::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(9)) return false; - if (!writer.writeTag("setPose",1,1)) return false; - if (!writer.writeString(id)) return false; - if (!writer.write(pose)) return false; - if (!writer.writeString(frame_name)) return false; - return true; +void WorldInterfaceServer_changeColor::init(const std::string& id, const Color& color) +{ + _return = false; + this->id = id; + this->color = color; } -bool WorldInterfaceServer_setPose::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; -} +class WorldInterfaceServer_setPose : + public yarp::os::Portable +{ +public: + std::string id; + Pose pose; + std::string frame_name; + bool _return; + void init(const std::string& id, const Pose& pose, const std::string& frame_name); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; -void WorldInterfaceServer_setPose::init(const std::string& id, const Pose& pose, const std::string& frame_name) { - _return = false; - this->id = id; - this->pose = pose; - this->frame_name = frame_name; +bool WorldInterfaceServer_setPose::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(9)) { + return false; + } + if (!writer.writeTag("setPose", 1, 1)) { + return false; + } + if (!writer.writeString(id)) { + return false; + } + if (!writer.write(pose)) { + return false; + } + if (!writer.writeString(frame_name)) { + return false; + } + return true; } -bool WorldInterfaceServer_enableGravity::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(3)) return false; - if (!writer.writeTag("enableGravity",1,1)) return false; - if (!writer.writeString(id)) return false; - if (!writer.writeBool(enable)) return false; - return true; +bool WorldInterfaceServer_setPose::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readBool(_return)) { + reader.fail(); + return false; + } + return true; } -bool WorldInterfaceServer_enableGravity::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; +void WorldInterfaceServer_setPose::init(const std::string& id, const Pose& pose, const std::string& frame_name) +{ + _return = false; + this->id = id; + this->pose = pose; + this->frame_name = frame_name; } -void WorldInterfaceServer_enableGravity::init(const std::string& id, const bool enable) { - _return = false; - this->id = id; - this->enable = enable; -} +class WorldInterfaceServer_enableGravity : + public yarp::os::Portable +{ +public: + std::string id; + bool enable; + bool _return; + void init(const std::string& id, const bool enable); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; -bool WorldInterfaceServer_enableCollision::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(3)) return false; - if (!writer.writeTag("enableCollision",1,1)) return false; - if (!writer.writeString(id)) return false; - if (!writer.writeBool(enable)) return false; - return true; +bool WorldInterfaceServer_enableGravity::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeTag("enableGravity", 1, 1)) { + return false; + } + if (!writer.writeString(id)) { + return false; + } + if (!writer.writeBool(enable)) { + return false; + } + return true; } -bool WorldInterfaceServer_enableCollision::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; +bool WorldInterfaceServer_enableGravity::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readBool(_return)) { + reader.fail(); + return false; + } + return true; } -void WorldInterfaceServer_enableCollision::init(const std::string& id, const bool enable) { - _return = false; - this->id = id; - this->enable = enable; +void WorldInterfaceServer_enableGravity::init(const std::string& id, const bool enable) +{ + _return = false; + this->id = id; + this->enable = enable; } -bool WorldInterfaceServer_getPose::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(3)) return false; - if (!writer.writeTag("getPose",1,1)) return false; - if (!writer.writeString(id)) return false; - if (!writer.writeString(frame_name)) return false; - return true; -} +class WorldInterfaceServer_enableCollision : + public yarp::os::Portable +{ +public: + std::string id; + bool enable; + bool _return; + void init(const std::string& id, const bool enable); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; -bool WorldInterfaceServer_getPose::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.read(_return)) { - reader.fail(); - return false; - } - return true; +bool WorldInterfaceServer_enableCollision::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeTag("enableCollision", 1, 1)) { + return false; + } + if (!writer.writeString(id)) { + return false; + } + if (!writer.writeBool(enable)) { + return false; + } + return true; } -void WorldInterfaceServer_getPose::init(const std::string& id, const std::string& frame_name) { - this->id = id; - this->frame_name = frame_name; +bool WorldInterfaceServer_enableCollision::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readBool(_return)) { + reader.fail(); + return false; + } + return true; } -bool WorldInterfaceServer_loadModelFromFile::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("loadModelFromFile",1,1)) return false; - if (!writer.writeString(filename)) return false; - return true; +void WorldInterfaceServer_enableCollision::init(const std::string& id, const bool enable) +{ + _return = false; + this->id = id; + this->enable = enable; } -bool WorldInterfaceServer_loadModelFromFile::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; -} +class WorldInterfaceServer_getPose : + public yarp::os::Portable +{ +public: + std::string id; + std::string frame_name; + Pose _return; + void init(const std::string& id, const std::string& frame_name); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; -void WorldInterfaceServer_loadModelFromFile::init(const std::string& filename) { - _return = false; - this->filename = filename; +bool WorldInterfaceServer_getPose::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(3)) { + return false; + } + if (!writer.writeTag("getPose", 1, 1)) { + return false; + } + if (!writer.writeString(id)) { + return false; + } + if (!writer.writeString(frame_name)) { + return false; + } + return true; } -bool WorldInterfaceServer_deleteObject::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("deleteObject",1,1)) return false; - if (!writer.writeString(id)) return false; - return true; +bool WorldInterfaceServer_getPose::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.read(_return)) { + reader.fail(); + return false; + } + return true; } -bool WorldInterfaceServer_deleteObject::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; +void WorldInterfaceServer_getPose::init(const std::string& id, const std::string& frame_name) +{ + this->id = id; + this->frame_name = frame_name; } -void WorldInterfaceServer_deleteObject::init(const std::string& id) { - _return = false; - this->id = id; -} +class WorldInterfaceServer_loadModelFromFile : + public yarp::os::Portable +{ +public: + std::string filename; + bool _return; + void init(const std::string& filename); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; -bool WorldInterfaceServer_deleteAll::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(1)) return false; - if (!writer.writeTag("deleteAll",1,1)) return false; - return true; +bool WorldInterfaceServer_loadModelFromFile::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(2)) { + return false; + } + if (!writer.writeTag("loadModelFromFile", 1, 1)) { + return false; + } + if (!writer.writeString(filename)) { + return false; + } + return true; } -bool WorldInterfaceServer_deleteAll::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; +bool WorldInterfaceServer_loadModelFromFile::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readBool(_return)) { + reader.fail(); + return false; + } + return true; } -void WorldInterfaceServer_deleteAll::init() { - _return = false; +void WorldInterfaceServer_loadModelFromFile::init(const std::string& filename) +{ + _return = false; + this->filename = filename; } -bool WorldInterfaceServer_getList::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(1)) return false; - if (!writer.writeTag("getList",1,1)) return false; - return true; +class WorldInterfaceServer_deleteObject : + public yarp::os::Portable +{ +public: + std::string id; + bool _return; + void init(const std::string& id); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; + +bool WorldInterfaceServer_deleteObject::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(2)) { + return false; + } + if (!writer.writeTag("deleteObject", 1, 1)) { + return false; + } + if (!writer.writeString(id)) { + return false; + } + return true; } -bool WorldInterfaceServer_getList::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - { - _return.clear(); - uint32_t _size0; - yarp::os::idl::WireState _etype3; - reader.readListBegin(_etype3, _size0); - _return.resize(_size0); - uint32_t _i4; - for (_i4 = 0; _i4 < _size0; ++_i4) - { - if (!reader.readString(_return[_i4])) { +bool WorldInterfaceServer_deleteObject::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readBool(_return)) { reader.fail(); return false; - } } - reader.readListEnd(); - } - return true; + return true; } -void WorldInterfaceServer_getList::init() { +void WorldInterfaceServer_deleteObject::init(const std::string& id) +{ + _return = false; + this->id = id; } -bool WorldInterfaceServer_attach::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(3)) return false; - if (!writer.writeTag("attach",1,1)) return false; - if (!writer.writeString(id)) return false; - if (!writer.writeString(link_name)) return false; - return true; -} +class WorldInterfaceServer_deleteAll : + public yarp::os::Portable +{ +public: + bool _return; + void init(); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; -bool WorldInterfaceServer_attach::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; +bool WorldInterfaceServer_deleteAll::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeTag("deleteAll", 1, 1)) { + return false; + } + return true; } -void WorldInterfaceServer_attach::init(const std::string& id, const std::string& link_name) { - _return = false; - this->id = id; - this->link_name = link_name; +bool WorldInterfaceServer_deleteAll::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + if (!reader.readBool(_return)) { + reader.fail(); + return false; + } + return true; } -bool WorldInterfaceServer_detach::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("detach",1,1)) return false; - if (!writer.writeString(id)) return false; - return true; +void WorldInterfaceServer_deleteAll::init() +{ + _return = false; } -bool WorldInterfaceServer_detach::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; +class WorldInterfaceServer_getList : + public yarp::os::Portable +{ +public: + std::vector _return; + void init(); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; + +bool WorldInterfaceServer_getList::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeTag("getList", 1, 1)) { + return false; + } + return true; } -void WorldInterfaceServer_detach::init(const std::string& id) { - _return = false; - this->id = id; +bool WorldInterfaceServer_getList::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { + return false; + } + _return.clear(); + uint32_t _size0; + yarp::os::idl::WireState _etype3; + reader.readListBegin(_etype3, _size0); + _return.resize(_size0); + for (auto& _elem4 : _return) { + if (!reader.readString(_elem4)) { + reader.fail(); + return false; + } + } + reader.readListEnd(); + return true; } -bool WorldInterfaceServer_rename::write(yarp::os::ConnectionWriter& connection) const { - yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(3)) return false; - if (!writer.writeTag("rename",1,1)) return false; - if (!writer.writeString(old_name)) return false; - if (!writer.writeString(new_name)) return false; - return true; +void WorldInterfaceServer_getList::init() +{ } -bool WorldInterfaceServer_rename::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - if (!reader.readListReturn()) return false; - if (!reader.readBool(_return)) { - reader.fail(); - return false; - } - return true; -} - -void WorldInterfaceServer_rename::init(const std::string& old_name, const std::string& new_name) { - _return = false; - this->old_name = old_name; - this->new_name = new_name; -} - -WorldInterfaceServer::WorldInterfaceServer() { - yarp().setOwner(*this); -} -std::string WorldInterfaceServer::makeSphere(const double radius, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) { - std::string _return = ""; - WorldInterfaceServer_makeSphere helper; - helper.init(radius,pose,color,frame_name,object_name,gravity_enable,collision_enable); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","std::string WorldInterfaceServer::makeSphere(const double radius, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -std::string WorldInterfaceServer::makeBox(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) { - std::string _return = ""; - WorldInterfaceServer_makeBox helper; - helper.init(width,height,thickness,pose,color,frame_name,object_name,gravity_enable,collision_enable); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","std::string WorldInterfaceServer::makeBox(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -std::string WorldInterfaceServer::makeCylinder(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) { - std::string _return = ""; - WorldInterfaceServer_makeCylinder helper; - helper.init(radius,length,pose,color,frame_name,object_name,gravity_enable,collision_enable); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","std::string WorldInterfaceServer::makeCylinder(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -std::string WorldInterfaceServer::makeFrame(const double size, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) { - std::string _return = ""; - WorldInterfaceServer_makeFrame helper; - helper.init(size,pose,color,frame_name,object_name,gravity_enable,collision_enable); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","std::string WorldInterfaceServer::makeFrame(const double size, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::changeColor(const std::string& id, const Color& color) { - bool _return = false; - WorldInterfaceServer_changeColor helper; - helper.init(id,color); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::changeColor(const std::string& id, const Color& color)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::setPose(const std::string& id, const Pose& pose, const std::string& frame_name) { - bool _return = false; - WorldInterfaceServer_setPose helper; - helper.init(id,pose,frame_name); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::setPose(const std::string& id, const Pose& pose, const std::string& frame_name)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::enableGravity(const std::string& id, const bool enable) { - bool _return = false; - WorldInterfaceServer_enableGravity helper; - helper.init(id,enable); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::enableGravity(const std::string& id, const bool enable)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::enableCollision(const std::string& id, const bool enable) { - bool _return = false; - WorldInterfaceServer_enableCollision helper; - helper.init(id,enable); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::enableCollision(const std::string& id, const bool enable)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -Pose WorldInterfaceServer::getPose(const std::string& id, const std::string& frame_name) { - Pose _return; - WorldInterfaceServer_getPose helper; - helper.init(id,frame_name); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","Pose WorldInterfaceServer::getPose(const std::string& id, const std::string& frame_name)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::loadModelFromFile(const std::string& filename) { - bool _return = false; - WorldInterfaceServer_loadModelFromFile helper; - helper.init(filename); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::loadModelFromFile(const std::string& filename)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::deleteObject(const std::string& id) { - bool _return = false; - WorldInterfaceServer_deleteObject helper; - helper.init(id); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::deleteObject(const std::string& id)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::deleteAll() { - bool _return = false; - WorldInterfaceServer_deleteAll helper; - helper.init(); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::deleteAll()"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -std::vector WorldInterfaceServer::getList() { - std::vector _return; - WorldInterfaceServer_getList helper; - helper.init(); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","std::vector WorldInterfaceServer::getList()"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::attach(const std::string& id, const std::string& link_name) { - bool _return = false; - WorldInterfaceServer_attach helper; - helper.init(id,link_name); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::attach(const std::string& id, const std::string& link_name)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::detach(const std::string& id) { - bool _return = false; - WorldInterfaceServer_detach helper; - helper.init(id); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::detach(const std::string& id)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} -bool WorldInterfaceServer::rename(const std::string& old_name, const std::string& new_name) { - bool _return = false; - WorldInterfaceServer_rename helper; - helper.init(old_name,new_name); - if (!yarp().canWrite()) { - yError("Missing server method '%s'?","bool WorldInterfaceServer::rename(const std::string& old_name, const std::string& new_name)"); - } - bool ok = yarp().write(helper,helper); - return ok?helper._return:_return; -} - -bool WorldInterfaceServer::read(yarp::os::ConnectionReader& connection) { - yarp::os::idl::WireReader reader(connection); - reader.expectAccept(); - if (!reader.readListHeader()) { reader.fail(); return false; } - std::string tag = reader.readTag(); - bool direct = (tag=="__direct__"); - if (direct) tag = reader.readTag(); - while (!reader.isError()) { - // TODO: use quick lookup, this is just a test - if (tag == "makeSphere") { - double radius; - Pose pose; - Color color; - std::string frame_name; - std::string object_name; - bool gravity_enable; - bool collision_enable; - if (!reader.readFloat64(radius)) { - reader.fail(); - return false; - } - if (!reader.read(pose)) { - reader.fail(); - return false; - } - if (!reader.read(color)) { - reader.fail(); - return false; - } - if (!reader.readString(frame_name)) { - frame_name = ""; - } - if (!reader.readString(object_name)) { - object_name = ""; - } - if (!reader.readBool(gravity_enable)) { - gravity_enable = 0; - } - if (!reader.readBool(collision_enable)) { - collision_enable = 1; - } - std::string _return; - _return = makeSphere(radius,pose,color,frame_name,object_name,gravity_enable,collision_enable); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "makeBox") { - double width; - double height; - double thickness; - Pose pose; - Color color; - std::string frame_name; - std::string object_name; - bool gravity_enable; - bool collision_enable; - if (!reader.readFloat64(width)) { - reader.fail(); - return false; - } - if (!reader.readFloat64(height)) { - reader.fail(); - return false; - } - if (!reader.readFloat64(thickness)) { - reader.fail(); - return false; - } - if (!reader.read(pose)) { - reader.fail(); - return false; - } - if (!reader.read(color)) { - reader.fail(); - return false; - } - if (!reader.readString(frame_name)) { - frame_name = ""; - } - if (!reader.readString(object_name)) { - object_name = ""; - } - if (!reader.readBool(gravity_enable)) { - gravity_enable = 0; - } - if (!reader.readBool(collision_enable)) { - collision_enable = 1; - } - std::string _return; - _return = makeBox(width,height,thickness,pose,color,frame_name,object_name,gravity_enable,collision_enable); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "makeCylinder") { - double radius; - double length; - Pose pose; - Color color; - std::string frame_name; - std::string object_name; - bool gravity_enable; - bool collision_enable; - if (!reader.readFloat64(radius)) { - reader.fail(); - return false; - } - if (!reader.readFloat64(length)) { - reader.fail(); +class WorldInterfaceServer_attach : + public yarp::os::Portable +{ +public: + std::string id; + std::string link_name; + bool _return; + void init(const std::string& id, const std::string& link_name); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; + +bool WorldInterfaceServer_attach::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(3)) { return false; - } - if (!reader.read(pose)) { - reader.fail(); + } + if (!writer.writeTag("attach", 1, 1)) { return false; - } - if (!reader.read(color)) { - reader.fail(); + } + if (!writer.writeString(id)) { return false; - } - if (!reader.readString(frame_name)) { - frame_name = ""; - } - if (!reader.readString(object_name)) { - object_name = ""; - } - if (!reader.readBool(gravity_enable)) { - gravity_enable = 0; - } - if (!reader.readBool(collision_enable)) { - collision_enable = 1; - } - std::string _return; - _return = makeCylinder(radius,length,pose,color,frame_name,object_name,gravity_enable,collision_enable); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "makeFrame") { - double size; - Pose pose; - Color color; - std::string frame_name; - std::string object_name; - bool gravity_enable; - bool collision_enable; - if (!reader.readFloat64(size)) { - reader.fail(); + } + if (!writer.writeString(link_name)) { return false; - } - if (!reader.read(pose)) { - reader.fail(); + } + return true; +} + +bool WorldInterfaceServer_attach::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { return false; - } - if (!reader.read(color)) { + } + if (!reader.readBool(_return)) { reader.fail(); return false; - } - if (!reader.readString(frame_name)) { - frame_name = ""; - } - if (!reader.readString(object_name)) { - object_name = ""; - } - if (!reader.readBool(gravity_enable)) { - gravity_enable = 0; - } - if (!reader.readBool(collision_enable)) { - collision_enable = 1; - } - std::string _return; - _return = makeFrame(size,pose,color,frame_name,object_name,gravity_enable,collision_enable); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeString(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "changeColor") { - std::string id; - Color color; - if (!reader.readString(id)) { - reader.fail(); + } + return true; +} + +void WorldInterfaceServer_attach::init(const std::string& id, const std::string& link_name) +{ + _return = false; + this->id = id; + this->link_name = link_name; +} + +class WorldInterfaceServer_detach : + public yarp::os::Portable +{ +public: + std::string id; + bool _return; + void init(const std::string& id); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; + +bool WorldInterfaceServer_detach::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(2)) { return false; - } - if (!reader.read(color)) { - reader.fail(); + } + if (!writer.writeTag("detach", 1, 1)) { return false; - } - bool _return; - _return = changeColor(id,color); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "setPose") { - std::string id; - Pose pose; - std::string frame_name; - if (!reader.readString(id)) { - reader.fail(); + } + if (!writer.writeString(id)) { return false; - } - if (!reader.read(pose)) { - reader.fail(); + } + return true; +} + +bool WorldInterfaceServer_detach::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { return false; - } - if (!reader.readString(frame_name)) { - frame_name = ""; - } - bool _return; - _return = setPose(id,pose,frame_name); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "enableGravity") { - std::string id; - bool enable; - if (!reader.readString(id)) { + } + if (!reader.readBool(_return)) { reader.fail(); return false; - } - if (!reader.readBool(enable)) { - reader.fail(); + } + return true; +} + +void WorldInterfaceServer_detach::init(const std::string& id) +{ + _return = false; + this->id = id; +} + +class WorldInterfaceServer_rename : + public yarp::os::Portable +{ +public: + std::string old_name; + std::string new_name; + bool _return; + void init(const std::string& old_name, const std::string& new_name); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; +}; + +bool WorldInterfaceServer_rename::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(3)) { return false; - } - bool _return; - _return = enableGravity(id,enable); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "enableCollision") { - std::string id; - bool enable; - if (!reader.readString(id)) { - reader.fail(); + } + if (!writer.writeTag("rename", 1, 1)) { return false; - } - if (!reader.readBool(enable)) { - reader.fail(); + } + if (!writer.writeString(old_name)) { return false; - } - bool _return; - _return = enableCollision(id,enable); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "getPose") { - std::string id; - std::string frame_name; - if (!reader.readString(id)) { - reader.fail(); + } + if (!writer.writeString(new_name)) { return false; - } - if (!reader.readString(frame_name)) { - frame_name = ""; - } - Pose _return; - _return = getPose(id,frame_name); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(6)) return false; - if (!writer.write(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "loadModelFromFile") { - std::string filename; - if (!reader.readString(filename)) { - reader.fail(); + } + return true; +} + +bool WorldInterfaceServer_rename::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListReturn()) { return false; - } - bool _return; - _return = loadModelFromFile(filename); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "deleteObject") { - std::string id; - if (!reader.readString(id)) { + } + if (!reader.readBool(_return)) { reader.fail(); return false; - } - bool _return; - _return = deleteObject(id); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "deleteAll") { - bool _return; - _return = deleteAll(); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "getList") { - std::vector _return; - _return = getList(); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - { - if (!writer.writeListBegin(BOTTLE_TAG_STRING, static_cast(_return.size()))) return false; - std::vector ::const_iterator _iter5; - for (_iter5 = _return.begin(); _iter5 != _return.end(); ++_iter5) - { - if (!writer.writeString((*_iter5))) return false; - } - if (!writer.writeListEnd()) return false; + } + return true; +} + +void WorldInterfaceServer_rename::init(const std::string& old_name, const std::string& new_name) +{ + _return = false; + this->old_name = old_name; + this->new_name = new_name; +} + +// Constructor +WorldInterfaceServer::WorldInterfaceServer() +{ + yarp().setOwner(*this); +} + +std::string WorldInterfaceServer::makeSphere(const double radius, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) +{ + std::string _return = ""; + WorldInterfaceServer_makeSphere helper; + helper.init(radius,pose,color,frame_name,object_name,gravity_enable,collision_enable); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "std::string WorldInterfaceServer::makeSphere(const double radius, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +std::string WorldInterfaceServer::makeBox(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) +{ + std::string _return = ""; + WorldInterfaceServer_makeBox helper; + helper.init(width,height,thickness,pose,color,frame_name,object_name,gravity_enable,collision_enable); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "std::string WorldInterfaceServer::makeBox(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +std::string WorldInterfaceServer::makeCylinder(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) +{ + std::string _return = ""; + WorldInterfaceServer_makeCylinder helper; + helper.init(radius,length,pose,color,frame_name,object_name,gravity_enable,collision_enable); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "std::string WorldInterfaceServer::makeCylinder(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +std::string WorldInterfaceServer::makeFrame(const double size, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable) +{ + std::string _return = ""; + WorldInterfaceServer_makeFrame helper; + helper.init(size,pose,color,frame_name,object_name,gravity_enable,collision_enable); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "std::string WorldInterfaceServer::makeFrame(const double size, const Pose& pose, const Color& color, const std::string& frame_name, const std::string& object_name, const bool gravity_enable, const bool collision_enable)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::changeColor(const std::string& id, const Color& color) +{ + bool _return = false; + WorldInterfaceServer_changeColor helper; + helper.init(id,color); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::changeColor(const std::string& id, const Color& color)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::setPose(const std::string& id, const Pose& pose, const std::string& frame_name) +{ + bool _return = false; + WorldInterfaceServer_setPose helper; + helper.init(id,pose,frame_name); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::setPose(const std::string& id, const Pose& pose, const std::string& frame_name)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::enableGravity(const std::string& id, const bool enable) +{ + bool _return = false; + WorldInterfaceServer_enableGravity helper; + helper.init(id,enable); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::enableGravity(const std::string& id, const bool enable)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::enableCollision(const std::string& id, const bool enable) +{ + bool _return = false; + WorldInterfaceServer_enableCollision helper; + helper.init(id,enable); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::enableCollision(const std::string& id, const bool enable)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +Pose WorldInterfaceServer::getPose(const std::string& id, const std::string& frame_name) +{ + Pose _return; + WorldInterfaceServer_getPose helper; + helper.init(id,frame_name); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "Pose WorldInterfaceServer::getPose(const std::string& id, const std::string& frame_name)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::loadModelFromFile(const std::string& filename) +{ + bool _return = false; + WorldInterfaceServer_loadModelFromFile helper; + helper.init(filename); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::loadModelFromFile(const std::string& filename)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::deleteObject(const std::string& id) +{ + bool _return = false; + WorldInterfaceServer_deleteObject helper; + helper.init(id); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::deleteObject(const std::string& id)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::deleteAll() +{ + bool _return = false; + WorldInterfaceServer_deleteAll helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::deleteAll()"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +std::vector WorldInterfaceServer::getList() +{ + std::vector _return; + WorldInterfaceServer_getList helper; + helper.init(); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "std::vector WorldInterfaceServer::getList()"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::attach(const std::string& id, const std::string& link_name) +{ + bool _return = false; + WorldInterfaceServer_attach helper; + helper.init(id,link_name); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::attach(const std::string& id, const std::string& link_name)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::detach(const std::string& id) +{ + bool _return = false; + WorldInterfaceServer_detach helper; + helper.init(id); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::detach(const std::string& id)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +bool WorldInterfaceServer::rename(const std::string& old_name, const std::string& new_name) +{ + bool _return = false; + WorldInterfaceServer_rename helper; + helper.init(old_name,new_name); + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", "bool WorldInterfaceServer::rename(const std::string& old_name, const std::string& new_name)"); + } + bool ok = yarp().write(helper, helper); + return ok ? helper._return : _return; +} + +// help method +std::vector WorldInterfaceServer::help(const std::string& functionName) +{ + bool showAll = (functionName == "--all"); + std::vector helpString; + if (showAll) { + helpString.emplace_back("*** Available commands:"); + helpString.emplace_back("makeSphere"); + helpString.emplace_back("makeBox"); + helpString.emplace_back("makeCylinder"); + helpString.emplace_back("makeFrame"); + helpString.emplace_back("changeColor"); + helpString.emplace_back("setPose"); + helpString.emplace_back("enableGravity"); + helpString.emplace_back("enableCollision"); + helpString.emplace_back("getPose"); + helpString.emplace_back("loadModelFromFile"); + helpString.emplace_back("deleteObject"); + helpString.emplace_back("deleteAll"); + helpString.emplace_back("getList"); + helpString.emplace_back("attach"); + helpString.emplace_back("detach"); + helpString.emplace_back("rename"); + helpString.emplace_back("help"); + } else { + if (functionName == "makeSphere") { + helpString.emplace_back("std::string makeSphere(const double radius, const Pose& pose, const Color& color, const std::string& frame_name = \"\", const std::string& object_name = \"\", const bool gravity_enable = 0, const bool collision_enable = 1) "); + helpString.emplace_back("Make a sphere. "); + helpString.emplace_back("@param radius radius of the sphere [m] "); + helpString.emplace_back("@param pose pose of the sphere [m] "); + helpString.emplace_back("@param color color of the sphere "); + helpString.emplace_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); + helpString.emplace_back("@param object_name (optional) assigns a name to the object. "); + helpString.emplace_back("@param gravity_enable (optional) enables gravity (default false) "); + helpString.emplace_back("@param collision_enable (optional) enables collision (default true) "); + helpString.emplace_back("@return returns a string that contains the name of the object in the world "); } - } - reader.accept(); - return true; - } - if (tag == "attach") { - std::string id; - std::string link_name; - if (!reader.readString(id)) { - reader.fail(); - return false; - } - if (!reader.readString(link_name)) { - reader.fail(); - return false; - } - bool _return; - _return = attach(id,link_name); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "detach") { - std::string id; - if (!reader.readString(id)) { - reader.fail(); - return false; - } - bool _return; - _return = detach(id); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "rename") { - std::string old_name; - std::string new_name; - if (!reader.readString(old_name)) { - reader.fail(); - return false; - } - if (!reader.readString(new_name)) { + if (functionName == "makeBox") { + helpString.emplace_back("std::string makeBox(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name = \"\", const std::string& object_name = \"\", const bool gravity_enable = 0, const bool collision_enable = 1) "); + helpString.emplace_back("Make a box. "); + helpString.emplace_back("@param width box width [m] "); + helpString.emplace_back("@param height box height[m] "); + helpString.emplace_back("@param thickness box thickness [m] "); + helpString.emplace_back("@param pose pose of the box [m] "); + helpString.emplace_back("@param color color of the box "); + helpString.emplace_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); + helpString.emplace_back("@param object_name (optional) assigns a name to the object. "); + helpString.emplace_back("@param gravity_enable (optional) enables gravity (default false) "); + helpString.emplace_back("@param collision_enable (optional) enables collision (default true) "); + helpString.emplace_back("@return returns a string that contains the name of the object in the world "); + } + if (functionName == "makeCylinder") { + helpString.emplace_back("std::string makeCylinder(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name = \"\", const std::string& object_name = \"\", const bool gravity_enable = 0, const bool collision_enable = 1) "); + helpString.emplace_back("Make a cylinder. "); + helpString.emplace_back("@param radius radius of the cylinder [m] "); + helpString.emplace_back("@param length lenght of the cylinder [m] "); + helpString.emplace_back("@param pose pose of the cylinder [m] "); + helpString.emplace_back("@param color color of the cylinder "); + helpString.emplace_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); + helpString.emplace_back("@param object_name (optional) assigns a name to the object. "); + helpString.emplace_back("@param gravity_enable (optional) enables gravity (default false) "); + helpString.emplace_back("@param collision_enable (optional) enables collision (default true) "); + helpString.emplace_back("@return returns a string that contains the name of the object in the world "); + } + if (functionName == "makeFrame") { + helpString.emplace_back("std::string makeFrame(const double size, const Pose& pose, const Color& color, const std::string& frame_name = \"\", const std::string& object_name = \"\", const bool gravity_enable = 0, const bool collision_enable = 1) "); + helpString.emplace_back("Make a reference frame. "); + helpString.emplace_back("@param size size of the frame [m] "); + helpString.emplace_back("@param pose pose of the frame [m] "); + helpString.emplace_back("@param color color of the frame "); + helpString.emplace_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); + helpString.emplace_back("@param object_name (optional) assigns a name to the object. "); + helpString.emplace_back("@param gravity_enable (optional) enables gravity (default false) "); + helpString.emplace_back("@param collision_enable (optional) enables collision (default true) "); + helpString.emplace_back("@return returns a string that contains the name of the object in the world "); + } + if (functionName == "changeColor") { + helpString.emplace_back("bool changeColor(const std::string& id, const Color& color) "); + helpString.emplace_back("Change the color of an object "); + helpString.emplace_back("@param id object id "); + helpString.emplace_back("@param color color of the frame "); + helpString.emplace_back("@return returns true or false on success failure "); + } + if (functionName == "setPose") { + helpString.emplace_back("bool setPose(const std::string& id, const Pose& pose, const std::string& frame_name = \"\") "); + helpString.emplace_back("Set new object pose. "); + helpString.emplace_back("@param id object id "); + helpString.emplace_back("@param pose new pose "); + helpString.emplace_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); + helpString.emplace_back("@return returns true or false on success failure "); + } + if (functionName == "enableGravity") { + helpString.emplace_back("bool enableGravity(const std::string& id, const bool enable) "); + helpString.emplace_back("Enable/disables gravity for an object "); + helpString.emplace_back("@param id object id "); + helpString.emplace_back("@param enable 1 to enable gravity, 0 otherwise "); + helpString.emplace_back("@return returns true or false on success failure "); + } + if (functionName == "enableCollision") { + helpString.emplace_back("bool enableCollision(const std::string& id, const bool enable) "); + helpString.emplace_back("Enable/disables collision detection for an object "); + helpString.emplace_back("@param id object id "); + helpString.emplace_back("@param enable 1 to enable collision detection, 0 otherwise "); + helpString.emplace_back("@return returns true or false on success failure "); + } + if (functionName == "getPose") { + helpString.emplace_back("Pose getPose(const std::string& id, const std::string& frame_name = \"\") "); + helpString.emplace_back("Get object pose. "); + helpString.emplace_back("@param id string that identifies object in gazebo (returned after creation) "); + helpString.emplace_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); + helpString.emplace_back("@return returns value of the pose in the world reference frame "); + } + if (functionName == "loadModelFromFile") { + helpString.emplace_back("bool loadModelFromFile(const std::string& filename) "); + helpString.emplace_back("Load a model from file. "); + helpString.emplace_back("@param id string that specifies the name of the model "); + helpString.emplace_back("@return returns true/false on success failure. "); + } + if (functionName == "deleteObject") { + helpString.emplace_back("bool deleteObject(const std::string& id) "); + helpString.emplace_back("Delete an object. "); + helpString.emplace_back("@param id string that identifies object in gazebo (returned after creation) "); + helpString.emplace_back("@return returns true/false on success failure. "); + } + if (functionName == "deleteAll") { + helpString.emplace_back("bool deleteAll() "); + helpString.emplace_back("Delete all objects in the world. "); + } + if (functionName == "getList") { + helpString.emplace_back("std::vector getList() "); + helpString.emplace_back("List id of all objects that have been added to the world. "); + helpString.emplace_back("@return return a list of string containing the id of the objects "); + } + if (functionName == "attach") { + helpString.emplace_back("bool attach(const std::string& id, const std::string& link_name) "); + helpString.emplace_back("Attach an object to a link of the robot. "); + helpString.emplace_back("@param id string that identifies object in gazebo (returned after creation) "); + helpString.emplace_back("@param link_name name of a link of the robot "); + helpString.emplace_back("@return true if success, false otherwise "); + } + if (functionName == "detach") { + helpString.emplace_back("bool detach(const std::string& id) "); + helpString.emplace_back("Detach a previously attached object. "); + helpString.emplace_back("@param id string that identifies object in gazebo (returned after creation) "); + helpString.emplace_back("@return true if success, false otherwise "); + } + if (functionName == "rename") { + helpString.emplace_back("bool rename(const std::string& old_name, const std::string& new_name) "); + helpString.emplace_back("Change the names of an object. "); + helpString.emplace_back("@param old_name string that identifies object in gazebo "); + helpString.emplace_back("@param new_name string that will be used as new name "); + helpString.emplace_back("@return true if success, false otherwise "); + } + if (functionName == "help") { + helpString.emplace_back("std::vector help(const std::string& functionName = \"--all\")"); + helpString.emplace_back("Return list of available commands, or help message for a specific function"); + helpString.emplace_back("@param functionName name of command for which to get a detailed description. If none or '--all' is provided, print list of available commands"); + helpString.emplace_back("@return list of strings (one string per line)"); + } + } + if (helpString.empty()) { + helpString.emplace_back("Command not found"); + } + return helpString; +} + +// read from ConnectionReader +bool WorldInterfaceServer::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + reader.expectAccept(); + if (!reader.readListHeader()) { reader.fail(); return false; - } - bool _return; - _return = rename(old_name,new_name); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(1)) return false; - if (!writer.writeBool(_return)) return false; - } - reader.accept(); - return true; - } - if (tag == "help") { - std::string functionName; - if (!reader.readString(functionName)) { - functionName = "--all"; - } - std::vector _return=help(functionName); - yarp::os::idl::WireWriter writer(reader); - if (!writer.isNull()) { - if (!writer.writeListHeader(2)) return false; - if (!writer.writeTag("many",1, 0)) return false; - if (!writer.writeListBegin(BOTTLE_TAG_INT32, static_cast(_return.size()))) return false; - std::vector ::iterator _iterHelp; - for (_iterHelp = _return.begin(); _iterHelp != _return.end(); ++_iterHelp) - { - if (!writer.writeString(*_iterHelp)) return false; - } - if (!writer.writeListEnd()) return false; - } - reader.accept(); - return true; - } - if (reader.noMore()) { reader.fail(); return false; } - std::string next_tag = reader.readTag(); - if (next_tag=="") break; - tag = tag + "_" + next_tag; - } - return false; -} - -std::vector WorldInterfaceServer::help(const std::string& functionName) { - bool showAll=(functionName=="--all"); - std::vector helpString; - if(showAll) { - helpString.push_back("*** Available commands:"); - helpString.push_back("makeSphere"); - helpString.push_back("makeBox"); - helpString.push_back("makeCylinder"); - helpString.push_back("makeFrame"); - helpString.push_back("changeColor"); - helpString.push_back("setPose"); - helpString.push_back("enableGravity"); - helpString.push_back("enableCollision"); - helpString.push_back("getPose"); - helpString.push_back("loadModelFromFile"); - helpString.push_back("deleteObject"); - helpString.push_back("deleteAll"); - helpString.push_back("getList"); - helpString.push_back("attach"); - helpString.push_back("detach"); - helpString.push_back("rename"); - helpString.push_back("help"); - } - else { - if (functionName=="makeSphere") { - helpString.push_back("std::string makeSphere(const double radius, const Pose& pose, const Color& color, const std::string& frame_name = \"\", const std::string& object_name = \"\", const bool gravity_enable = 0, const bool collision_enable = 1) "); - helpString.push_back("Make a sphere. "); - helpString.push_back("@param radius radius of the sphere [m] "); - helpString.push_back("@param pose pose of the sphere [m] "); - helpString.push_back("@param color color of the sphere "); - helpString.push_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); - helpString.push_back("@param object_name (optional) assigns a name to the object. "); - helpString.push_back("@param gravity_enable (optional) enables gravity (default false) "); - helpString.push_back("@param collision_enable (optional) enables collision (default true) "); - helpString.push_back("@return returns a string that contains the name of the object in the world "); - } - if (functionName=="makeBox") { - helpString.push_back("std::string makeBox(const double width, const double height, const double thickness, const Pose& pose, const Color& color, const std::string& frame_name = \"\", const std::string& object_name = \"\", const bool gravity_enable = 0, const bool collision_enable = 1) "); - helpString.push_back("Make a box. "); - helpString.push_back("@param width box width [m] "); - helpString.push_back("@param height box height[m] "); - helpString.push_back("@param thickness box thickness [m] "); - helpString.push_back("@param pose pose of the box [m] "); - helpString.push_back("@param color color of the box "); - helpString.push_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); - helpString.push_back("@param object_name (optional) assigns a name to the object. "); - helpString.push_back("@param gravity_enable (optional) enables gravity (default false) "); - helpString.push_back("@param collision_enable (optional) enables collision (default true) "); - helpString.push_back("@return returns a string that contains the name of the object in the world "); - } - if (functionName=="makeCylinder") { - helpString.push_back("std::string makeCylinder(const double radius, const double length, const Pose& pose, const Color& color, const std::string& frame_name = \"\", const std::string& object_name = \"\", const bool gravity_enable = 0, const bool collision_enable = 1) "); - helpString.push_back("Make a cylinder. "); - helpString.push_back("@param radius radius of the cylinder [m] "); - helpString.push_back("@param length lenght of the cylinder [m] "); - helpString.push_back("@param pose pose of the cylinder [m] "); - helpString.push_back("@param color color of the cylinder "); - helpString.push_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); - helpString.push_back("@param object_name (optional) assigns a name to the object. "); - helpString.push_back("@param gravity_enable (optional) enables gravity (default false) "); - helpString.push_back("@param collision_enable (optional) enables collision (default true) "); - helpString.push_back("@return returns a string that contains the name of the object in the world "); - } - if (functionName=="makeFrame") { - helpString.push_back("std::string makeFrame(const double size, const Pose& pose, const Color& color, const std::string& frame_name = \"\", const std::string& object_name = \"\", const bool gravity_enable = 0, const bool collision_enable = 1) "); - helpString.push_back("Make a reference frame. "); - helpString.push_back("@param size size of the frame [m] "); - helpString.push_back("@param pose pose of the frame [m] "); - helpString.push_back("@param color color of the frame "); - helpString.push_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); - helpString.push_back("@param object_name (optional) assigns a name to the object. "); - helpString.push_back("@param gravity_enable (optional) enables gravity (default false) "); - helpString.push_back("@param collision_enable (optional) enables collision (default true) "); - helpString.push_back("@return returns a string that contains the name of the object in the world "); - } - if (functionName=="changeColor") { - helpString.push_back("bool changeColor(const std::string& id, const Color& color) "); - helpString.push_back("Change the color of an object "); - helpString.push_back("@param id object id "); - helpString.push_back("@param color color of the frame "); - helpString.push_back("@return returns true or false on success failure "); - } - if (functionName=="setPose") { - helpString.push_back("bool setPose(const std::string& id, const Pose& pose, const std::string& frame_name = \"\") "); - helpString.push_back("Set new object pose. "); - helpString.push_back("@param id object id "); - helpString.push_back("@param pose new pose "); - helpString.push_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); - helpString.push_back("@return returns true or false on success failure "); - } - if (functionName=="enableGravity") { - helpString.push_back("bool enableGravity(const std::string& id, const bool enable) "); - helpString.push_back("Enable/disables gravity for an object "); - helpString.push_back("@param id object id "); - helpString.push_back("@param enable 1 to enable gravity, 0 otherwise "); - helpString.push_back("@return returns true or false on success failure "); - } - if (functionName=="enableCollision") { - helpString.push_back("bool enableCollision(const std::string& id, const bool enable) "); - helpString.push_back("Enable/disables collision detection for an object "); - helpString.push_back("@param id object id "); - helpString.push_back("@param enable 1 to enable collision detection, 0 otherwise "); - helpString.push_back("@return returns true or false on success failure "); - } - if (functionName=="getPose") { - helpString.push_back("Pose getPose(const std::string& id, const std::string& frame_name = \"\") "); - helpString.push_back("Get object pose. "); - helpString.push_back("@param id string that identifies object in gazebo (returned after creation) "); - helpString.push_back("@param frame_name (optional) is specified, the pose will be relative to the specified fully scoped frame (e.g. MODEL_ID::FRAME_ID). Otherwise, world it will be used. "); - helpString.push_back("@return returns value of the pose in the world reference frame "); - } - if (functionName=="loadModelFromFile") { - helpString.push_back("bool loadModelFromFile(const std::string& filename) "); - helpString.push_back("Load a model from file. "); - helpString.push_back("@param id string that specifies the name of the model "); - helpString.push_back("@return returns true/false on success failure. "); - } - if (functionName=="deleteObject") { - helpString.push_back("bool deleteObject(const std::string& id) "); - helpString.push_back("Delete an object. "); - helpString.push_back("@param id string that identifies object in gazebo (returned after creation) "); - helpString.push_back("@return returns true/false on success failure. "); - } - if (functionName=="deleteAll") { - helpString.push_back("bool deleteAll() "); - helpString.push_back("Delete all objects in the world. "); - } - if (functionName=="getList") { - helpString.push_back("std::vector getList() "); - helpString.push_back("List id of all objects that have been added to the world. "); - helpString.push_back("@return return a list of string containing the id of the objects "); - } - if (functionName=="attach") { - helpString.push_back("bool attach(const std::string& id, const std::string& link_name) "); - helpString.push_back("Attach an object to a link of the robot. "); - helpString.push_back("@param id string that identifies object in gazebo (returned after creation) "); - helpString.push_back("@param link_name name of a link of the robot "); - helpString.push_back("@return true if success, false otherwise "); - } - if (functionName=="detach") { - helpString.push_back("bool detach(const std::string& id) "); - helpString.push_back("Detach a previously attached object. "); - helpString.push_back("@param id string that identifies object in gazebo (returned after creation) "); - helpString.push_back("@return true if success, false otherwise "); - } - if (functionName=="rename") { - helpString.push_back("bool rename(const std::string& old_name, const std::string& new_name) "); - helpString.push_back("Change the names of an object. "); - helpString.push_back("@param old_name string that identifies object in gazebo "); - helpString.push_back("@param new_name string that will be used as new name "); - helpString.push_back("@return true if success, false otherwise "); - } - if (functionName=="help") { - helpString.push_back("std::vector help(const std::string& functionName=\"--all\")"); - helpString.push_back("Return list of available commands, or help message for a specific function"); - helpString.push_back("@param functionName name of command for which to get a detailed description. If none or '--all' is provided, print list of available commands"); - helpString.push_back("@return list of strings (one string per line)"); - } - } - if ( helpString.empty()) helpString.push_back("Command not found"); - return helpString; -} -} // namespace + } + std::string tag = reader.readTag(); + bool direct = (tag == "__direct__"); + if (direct) { + tag = reader.readTag(); + } + while (!reader.isError()) { + if (tag == "makeSphere") { + double radius; + Pose pose; + Color color; + std::string frame_name; + std::string object_name; + bool gravity_enable; + bool collision_enable; + if (!reader.readFloat64(radius)) { + reader.fail(); + return false; + } + if (!reader.read(pose)) { + reader.fail(); + return false; + } + if (!reader.read(color)) { + reader.fail(); + return false; + } + if (!reader.readString(frame_name)) { + frame_name = ""; + } + if (!reader.readString(object_name)) { + object_name = ""; + } + if (!reader.readBool(gravity_enable)) { + gravity_enable = 0; + } + if (!reader.readBool(collision_enable)) { + collision_enable = 1; + } + std::string _return; + _return = makeSphere(radius,pose,color,frame_name,object_name,gravity_enable,collision_enable); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "makeBox") { + double width; + double height; + double thickness; + Pose pose; + Color color; + std::string frame_name; + std::string object_name; + bool gravity_enable; + bool collision_enable; + if (!reader.readFloat64(width)) { + reader.fail(); + return false; + } + if (!reader.readFloat64(height)) { + reader.fail(); + return false; + } + if (!reader.readFloat64(thickness)) { + reader.fail(); + return false; + } + if (!reader.read(pose)) { + reader.fail(); + return false; + } + if (!reader.read(color)) { + reader.fail(); + return false; + } + if (!reader.readString(frame_name)) { + frame_name = ""; + } + if (!reader.readString(object_name)) { + object_name = ""; + } + if (!reader.readBool(gravity_enable)) { + gravity_enable = 0; + } + if (!reader.readBool(collision_enable)) { + collision_enable = 1; + } + std::string _return; + _return = makeBox(width,height,thickness,pose,color,frame_name,object_name,gravity_enable,collision_enable); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "makeCylinder") { + double radius; + double length; + Pose pose; + Color color; + std::string frame_name; + std::string object_name; + bool gravity_enable; + bool collision_enable; + if (!reader.readFloat64(radius)) { + reader.fail(); + return false; + } + if (!reader.readFloat64(length)) { + reader.fail(); + return false; + } + if (!reader.read(pose)) { + reader.fail(); + return false; + } + if (!reader.read(color)) { + reader.fail(); + return false; + } + if (!reader.readString(frame_name)) { + frame_name = ""; + } + if (!reader.readString(object_name)) { + object_name = ""; + } + if (!reader.readBool(gravity_enable)) { + gravity_enable = 0; + } + if (!reader.readBool(collision_enable)) { + collision_enable = 1; + } + std::string _return; + _return = makeCylinder(radius,length,pose,color,frame_name,object_name,gravity_enable,collision_enable); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "makeFrame") { + double size; + Pose pose; + Color color; + std::string frame_name; + std::string object_name; + bool gravity_enable; + bool collision_enable; + if (!reader.readFloat64(size)) { + reader.fail(); + return false; + } + if (!reader.read(pose)) { + reader.fail(); + return false; + } + if (!reader.read(color)) { + reader.fail(); + return false; + } + if (!reader.readString(frame_name)) { + frame_name = ""; + } + if (!reader.readString(object_name)) { + object_name = ""; + } + if (!reader.readBool(gravity_enable)) { + gravity_enable = 0; + } + if (!reader.readBool(collision_enable)) { + collision_enable = 1; + } + std::string _return; + _return = makeFrame(size,pose,color,frame_name,object_name,gravity_enable,collision_enable); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeString(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "changeColor") { + std::string id; + Color color; + if (!reader.readString(id)) { + reader.fail(); + return false; + } + if (!reader.read(color)) { + reader.fail(); + return false; + } + bool _return; + _return = changeColor(id,color); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "setPose") { + std::string id; + Pose pose; + std::string frame_name; + if (!reader.readString(id)) { + reader.fail(); + return false; + } + if (!reader.read(pose)) { + reader.fail(); + return false; + } + if (!reader.readString(frame_name)) { + frame_name = ""; + } + bool _return; + _return = setPose(id,pose,frame_name); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "enableGravity") { + std::string id; + bool enable; + if (!reader.readString(id)) { + reader.fail(); + return false; + } + if (!reader.readBool(enable)) { + reader.fail(); + return false; + } + bool _return; + _return = enableGravity(id,enable); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "enableCollision") { + std::string id; + bool enable; + if (!reader.readString(id)) { + reader.fail(); + return false; + } + if (!reader.readBool(enable)) { + reader.fail(); + return false; + } + bool _return; + _return = enableCollision(id,enable); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "getPose") { + std::string id; + std::string frame_name; + if (!reader.readString(id)) { + reader.fail(); + return false; + } + if (!reader.readString(frame_name)) { + frame_name = ""; + } + Pose _return; + _return = getPose(id,frame_name); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(6)) { + return false; + } + if (!writer.write(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "loadModelFromFile") { + std::string filename; + if (!reader.readString(filename)) { + reader.fail(); + return false; + } + bool _return; + _return = loadModelFromFile(filename); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "deleteObject") { + std::string id; + if (!reader.readString(id)) { + reader.fail(); + return false; + } + bool _return; + _return = deleteObject(id); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "deleteAll") { + bool _return; + _return = deleteAll(); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "getList") { + std::vector _return; + _return = getList(); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeListBegin(BOTTLE_TAG_STRING, static_cast(_return.size()))) { + return false; + } + for (const auto& _item5 : _return) { + if (!writer.writeString(_item5)) { + return false; + } + } + if (!writer.writeListEnd()) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "attach") { + std::string id; + std::string link_name; + if (!reader.readString(id)) { + reader.fail(); + return false; + } + if (!reader.readString(link_name)) { + reader.fail(); + return false; + } + bool _return; + _return = attach(id,link_name); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "detach") { + std::string id; + if (!reader.readString(id)) { + reader.fail(); + return false; + } + bool _return; + _return = detach(id); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "rename") { + std::string old_name; + std::string new_name; + if (!reader.readString(old_name)) { + reader.fail(); + return false; + } + if (!reader.readString(new_name)) { + reader.fail(); + return false; + } + bool _return; + _return = rename(old_name,new_name); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(1)) { + return false; + } + if (!writer.writeBool(_return)) { + return false; + } + } + reader.accept(); + return true; + } + if (tag == "help") { + std::string functionName; + if (!reader.readString(functionName)) { + functionName = "--all"; + } + auto _return = help(functionName); + yarp::os::idl::WireWriter writer(reader); + if (!writer.isNull()) { + if (!writer.writeListHeader(2)) { + return false; + } + if (!writer.writeTag("many", 1, 0)) { + return false; + } + if (!writer.writeListBegin(BOTTLE_TAG_INT32, static_cast(_return.size()))) { + return false; + } + for (const auto& _ret : _return) { + if (!writer.writeString(_ret)) { + return false; + } + } + if (!writer.writeListEnd()) { + return false; + } + } + reader.accept(); + return true; + } + if (reader.noMore()) { + reader.fail(); + return false; + } + std::string next_tag = reader.readTag(); + if (next_tag == "") { + break; + } + tag.append("_").append(next_tag); + } + return false; +} +} // namespace GazeboYarpPlugins diff --git a/thrift/worldinterface/autogenerated/worldinterface_rpc_thrift.cmake b/thrift/worldinterface/autogenerated/worldinterface_rpc_thrift.cmake index bb60da889..f53a98592 100644 --- a/thrift/worldinterface/autogenerated/worldinterface_rpc_thrift.cmake +++ b/thrift/worldinterface/autogenerated/worldinterface_rpc_thrift.cmake @@ -1,4 +1,4 @@ -# Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) +# Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) # All rights reserved. # # This software may be modified and distributed under the terms of the From cb9952a8f7695be165d8bbfeda6da1442b7a7f83 Mon Sep 17 00:00:00 2001 From: Jason Chevrie Date: Wed, 16 Jan 2019 14:39:13 +0100 Subject: [PATCH 22/25] Fix invalid camera parameters in gazebo_depthCamera plugin. Fix `focalLengthX` and `focalLengthY` values. When asking the parameters through rpc (via the command `visr get intp`), the `focalLengthX` and `focalLengthY` parameters should be expressed in pixel size instead of meters. Fix typo. --- plugins/depthCamera/src/DepthCamera.cc | 2 +- plugins/depthCamera/src/DepthCameraDriver.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/plugins/depthCamera/src/DepthCamera.cc b/plugins/depthCamera/src/DepthCamera.cc index d210a3298..9ed496b85 100644 --- a/plugins/depthCamera/src/DepthCamera.cc +++ b/plugins/depthCamera/src/DepthCamera.cc @@ -94,7 +94,7 @@ void GazeboYarpDepthCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd } //Open the driver - //Force the device to be of type "gazebo_forcetorque" (it make sense? probably yes) + //Force the device to be of type "gazebo_depthCamera" (it make sense? probably yes) m_driverParameters.put("device","gazebo_depthCamera"); yDebug() << "CC: m_driverParameters:\t" << m_driverParameters.toString(); diff --git a/plugins/depthCamera/src/DepthCameraDriver.cpp b/plugins/depthCamera/src/DepthCameraDriver.cpp index e674f7a21..0c3f035d4 100644 --- a/plugins/depthCamera/src/DepthCameraDriver.cpp +++ b/plugins/depthCamera/src/DepthCameraDriver.cpp @@ -334,8 +334,8 @@ bool GazeboYarpDepthCameraDriver::getDepthIntrinsicParam(Property& intrinsic) distModel = camPtr->LensDistortion().get(); if(distModel) { - intrinsic.put("focalLengthX", camPtr->OgreCamera()->getFocalLength()); - intrinsic.put("focalLengthY", camPtr->OgreCamera()->getFocalLength() * camPtr->OgreCamera()->getAspectRatio()); + intrinsic.put("focalLengthX", 1. / camPtr->OgreCamera()->getPixelDisplayRatio()); + intrinsic.put("focalLengthY", 1. / camPtr->OgreCamera()->getPixelDisplayRatio()); #if GAZEBO_MAJOR_VERSION >= 8 intrinsic.put("k1", distModel->K1()); intrinsic.put("k2", distModel->K2()); From f51713bbfa86a91c65d5ef4e0f2e447790e7185c Mon Sep 17 00:00:00 2001 From: Jason Chevrie Date: Tue, 4 Jun 2019 17:16:17 +0200 Subject: [PATCH 23/25] Add missing `physFocalLength` parameter --- plugins/depthCamera/src/DepthCameraDriver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/plugins/depthCamera/src/DepthCameraDriver.cpp b/plugins/depthCamera/src/DepthCameraDriver.cpp index 0c3f035d4..acba60619 100644 --- a/plugins/depthCamera/src/DepthCameraDriver.cpp +++ b/plugins/depthCamera/src/DepthCameraDriver.cpp @@ -334,6 +334,7 @@ bool GazeboYarpDepthCameraDriver::getDepthIntrinsicParam(Property& intrinsic) distModel = camPtr->LensDistortion().get(); if(distModel) { + intrinsic.put("physFocalLength", 0.0); intrinsic.put("focalLengthX", 1. / camPtr->OgreCamera()->getPixelDisplayRatio()); intrinsic.put("focalLengthY", 1. / camPtr->OgreCamera()->getPixelDisplayRatio()); #if GAZEBO_MAJOR_VERSION >= 8 From f0bae32a14d6d045b437cf4fdc7d0dda647a37fb Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 1 Jul 2019 11:53:30 +0200 Subject: [PATCH 24/25] Add autogenerated thrift files --- thrift/clock/autogenerated/clock_rpc_index.txt | 2 ++ .../linkattacher/autogenerated/linkattacher_rpc_index.txt | 2 ++ .../autogenerated/worldinterface_rpc_index.txt | 6 ++++++ 3 files changed, 10 insertions(+) create mode 100644 thrift/clock/autogenerated/clock_rpc_index.txt create mode 100644 thrift/linkattacher/autogenerated/linkattacher_rpc_index.txt create mode 100644 thrift/worldinterface/autogenerated/worldinterface_rpc_index.txt diff --git a/thrift/clock/autogenerated/clock_rpc_index.txt b/thrift/clock/autogenerated/clock_rpc_index.txt new file mode 100644 index 000000000..4d96d0f6b --- /dev/null +++ b/thrift/clock/autogenerated/clock_rpc_index.txt @@ -0,0 +1,2 @@ +include/ClockServer.h +src/ClockServer.cpp diff --git a/thrift/linkattacher/autogenerated/linkattacher_rpc_index.txt b/thrift/linkattacher/autogenerated/linkattacher_rpc_index.txt new file mode 100644 index 000000000..6961f2f53 --- /dev/null +++ b/thrift/linkattacher/autogenerated/linkattacher_rpc_index.txt @@ -0,0 +1,2 @@ +include/LinkAttacherServer.h +src/LinkAttacherServer.cpp diff --git a/thrift/worldinterface/autogenerated/worldinterface_rpc_index.txt b/thrift/worldinterface/autogenerated/worldinterface_rpc_index.txt new file mode 100644 index 000000000..0b509ce7f --- /dev/null +++ b/thrift/worldinterface/autogenerated/worldinterface_rpc_index.txt @@ -0,0 +1,6 @@ +include/Pose.h +src/Pose.cpp +include/Color.h +src/Color.cpp +include/WorldInterfaceServer.h +src/WorldInterfaceServer.cpp From 147419edecd7684f4402979fd6ae2bc536849fcc Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 1 Jul 2019 11:53:47 +0200 Subject: [PATCH 25/25] Bump version and update ChangeLog document. --- CHANGELOG.md | 17 +++++++++++++++++ CMakeLists.txt | 4 ++-- 2 files changed, 19 insertions(+), 2 deletions(-) create mode 100644 CHANGELOG.md diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 000000000..a95e4fee0 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,17 @@ +# Changelog +All notable changes to this project will be documented in this file. + +The format of this document is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). + +## [3.2.0] - 2019-07-01 +### Added +- Add `resetSimulation` method to reset simulation state and time time, in the `gazebo_yarp_clock` RPC interface (https://github.com/robotology/gazebo-yarp-plugins/pull/345). +- Add `resetSimulationState` method to reset simulation state, but not the time, in the `gazebo_yarp_clock` RPC interface (https://github.com/robotology/gazebo-yarp-plugins/pull/424). +- Add `gazebo_yarp_doublelaser` plugin to simulate a double laser device (https://github.com/robotology/gazebo-yarp-plugins/pull/419). +- Add `gazebo_yarp_configurationoverride` plugin that can be attached to a model in order to override the [`sdf`](http://sdformat.org/spec) configuration of any other plugin that is attached to the same model (https://github.com/robotology/gazebo-yarp-plugins/pull/401). This plugin is particularly useful to set the initial configuration of a given model, and is documented in https://github.com/robotology/gazebo-yarp-plugins/blob/v3.2.0/plugins/configurationoverride/README.md . +- Add features in `gazebo_yarp_externalwrench` to apply multiple external wrenchs at the same time (https://github.com/robotology/gazebo-yarp-plugins/pull/418) . +- Add the `yarpConfigurationString` to configure the YARP plugins with a single `yarpConfigurationString` embedded in the SDF code of the plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/396). +- Add a ChangeLog document based on the format defined in [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). + +### Fixed +- Fix invalid camera parameters in `gazebo_depthCamera` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/408). diff --git a/CMakeLists.txt b/CMakeLists.txt index 02fcbbaa0..6294f8bcf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,8 +7,8 @@ PROJECT(GazeboYARPPlugins) # Project version set(${PROJECT_NAME}_MAJOR_VERSION 3) -set(${PROJECT_NAME}_MINOR_VERSION 1) -set(${PROJECT_NAME}_PATCH_VERSION 101) +set(${PROJECT_NAME}_MINOR_VERSION 2) +set(${PROJECT_NAME}_PATCH_VERSION 0) set(${PROJECT_NAME}_VERSION ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION})