diff --git a/mars.orogen b/mars.orogen old mode 100644 new mode 100755 index 98d3ebb7..4f935c86 --- a/mars.orogen +++ b/mars.orogen @@ -12,6 +12,7 @@ import_types_from "jointTypes.hpp" import_types_from "poseType.hpp" import_types_from "objectDetectionTypes.hpp" import_types_from "wrenchTypes.hpp" +import_types_from "poseUpdateType.hpp" import_types_from "base" @@ -350,7 +351,7 @@ task_context "EntityFakeDetection" do property("minVisibleVertices", "/uint16_t", 5). doc("Number of vertices that have to be in the viewing frustum to be counted as seen. Center is counted as vertex.") property("use_camera", "/bool", false). - doc("Whether the camera should be used.") + doc("Whether the camera should be used.") output_port('detectionArray', '/mars/Detection3DArray'). doc 'Puts out the entities in a ROS similar detection format.' @@ -364,6 +365,8 @@ task_context "RobotTeleportation" do property("robot_name", "/std/string"). doc("Name of the robot.") + property("scene_path", "/std/string"). + doc("Path to smurfs") property("reset_node_name", "/std/string"). doc("Name of the node that has to be resetted additionally.") property("position_mode", "/uint16_t", 1). @@ -385,6 +388,49 @@ task_context "RobotTeleportation" do end +task_context "AstronautLocalizerTask" do + subclasses "Plugin" + + property("entity_name", "/std/string", "astronaut"). + doc("Name of mars entity which represents astronaut") + + output_port("astronaut_pose", "/base/samples/RigidBodyState") +end + + +task_context "RepositionTask" do + # Repositions the named entities of the given scene in the current mars scene + subclasses "Plugin" + + property("entity_names", "/std/vector"). + doc("Name of parts to move.") + property("scene_path", "/std/string"). + doc("Path to smurf(a/s)") + property("ignore_position", "bool", false). + doc("Whether the position input shall be ignored") + + input_port('poses', '/std/vector'). + doc 'Poses relative to the parent link' + port_driven + + needs_configuration + +end + +task_context "PoseUpdateTask" do + # Repositions the named entities of the given scene in the current mars scene + subclasses "Plugin" + + input_port('single_pose_update', '/mars/PoseUpdate') + input_port('multi_pose_updates', '/mars/PoseUpdates') + + #input_port('single_relative_pose_update', '/mars/PoseUpdate') + #input_port('multi_relative_pose_updates', '/mars/PoseUpdates') + port_driven + + needs_configuration + + end task_context "ObjectHighlighter" do subclasses "Plugin" diff --git a/objectDetectionTypes.hpp b/objectDetectionTypes.hpp index d7bae9fc..52b837d5 100644 --- a/objectDetectionTypes.hpp +++ b/objectDetectionTypes.hpp @@ -7,6 +7,7 @@ #include // TODO 1 #include + namespace mars { @@ -68,4 +69,30 @@ namespace mars }; } + +// need to tell the stream_aligner/transformer that the timestamp of these types +// is hidden in "header.stamp", instead of the default "time" member variable +namespace aggregator { + inline base::Time determineTimestamp(const mars::Detection3DArray& sample) + { + return sample.header.stamp; + } + + inline base::Time determineTimestamp(const mars::Detection3D& sample) + { + return sample.header.stamp; + } + + inline base::Time determineTimestamp(const mars::PointCloud& sample) + { + return sample.header.stamp; + } + + inline base::Time determineTimestamp(const mars::Header& sample) + { + return sample.stamp; + } +} + + #endif diff --git a/poseUpdateType.hpp b/poseUpdateType.hpp new file mode 100644 index 00000000..ed60eea9 --- /dev/null +++ b/poseUpdateType.hpp @@ -0,0 +1,20 @@ +#ifndef MARS_POSITION_UPDATE_TYPE_HH +#define MARS_POSITION_UPDATE_TYPE_HH + +#include +#include + +#include + +namespace mars +{ + +struct PoseUpdate { + std::string entity_name; + base::Pose pose; +}; + +typedef std::vector PoseUpdates; + +} +#endif diff --git a/tasks/AstronautLocalizerTask.cpp b/tasks/AstronautLocalizerTask.cpp new file mode 100644 index 00000000..eaed1b7b --- /dev/null +++ b/tasks/AstronautLocalizerTask.cpp @@ -0,0 +1,70 @@ +/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */ + +#include "AstronautLocalizerTask.hpp" +#include +#include +#include +#include +#include + +using namespace mars; + +AstronautLocalizerTask::AstronautLocalizerTask(std::string const& name) + : AstronautLocalizerTaskBase(name) +{ +} + +AstronautLocalizerTask::~AstronautLocalizerTask() +{ +} + + + +/// The following lines are template definitions for the various state machine +// hooks defined by Orocos::RTT. See AstronautLocalizerTask.hpp for more detailed +// documentation about them. + +bool AstronautLocalizerTask::configureHook() +{ + if (! AstronautLocalizerTaskBase::configureHook()) + return false; + return true; +} +bool AstronautLocalizerTask::startHook() +{ + if (! AstronautLocalizerTaskBase::startHook()) + return false; + return true; +} +void AstronautLocalizerTask::updateHook() +{ + AstronautLocalizerTaskBase::updateHook(); + + sim::SimEntity* ent = control->entities->getEntity(_entity_name.get()); + if (ent != nullptr) { + configmaps::ConfigMap cfg = ent->getConfig(); + base::samples::RigidBodyState p; + p.position[0] = cfg["position"][0]; + p.position[1] = cfg["position"][1]; + p.position[2] = cfg["position"][2]; + + p.orientation = Eigen::Quaterniond(cfg["rotation"][0], cfg["rotation"][1], cfg["rotation"][2], cfg["rotation"][3]); + + p.sourceFrame = "astronaut_feet"; + p.targetFrame = "world"; + + _astronaut_pose.write(p); + } +} +void AstronautLocalizerTask::errorHook() +{ + AstronautLocalizerTaskBase::errorHook(); +} +void AstronautLocalizerTask::stopHook() +{ + AstronautLocalizerTaskBase::stopHook(); +} +void AstronautLocalizerTask::cleanupHook() +{ + AstronautLocalizerTaskBase::cleanupHook(); +} diff --git a/tasks/AstronautLocalizerTask.hpp b/tasks/AstronautLocalizerTask.hpp new file mode 100644 index 00000000..7cf4ca6b --- /dev/null +++ b/tasks/AstronautLocalizerTask.hpp @@ -0,0 +1,103 @@ +/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */ + +#ifndef MARS_AstronautLocalizerTASK_TASK_HPP +#define MARS_AstronautLocalizerTASK_TASK_HPP + +#include "mars/AstronautLocalizerTaskBase.hpp" + +namespace mars{ + + /*! \class AstronautLocalizerTask + * \brief The task context provides and requires services. It uses an ExecutionEngine to perform its functions. + * Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. + * In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow. + * + * \details + * The name of a TaskContext is primarily defined via: + \verbatim + deployment 'deployment_name' + task('custom_task_name','mars::AstronautLocalizerTask') + end + \endverbatim + * It can be dynamically adapted when the deployment is called with a prefix argument. + */ + class AstronautLocalizerTask : public AstronautLocalizerTaskBase + { + friend class AstronautLocalizerTaskBase; + protected: + + + + public: + /** TaskContext constructor for AstronautLocalizerTask + * \param name Name of the task. This name needs to be unique to make it identifiable via nameservices. + * \param initial_state The initial TaskState of the TaskContext. Default is Stopped state. + */ + AstronautLocalizerTask(std::string const& name = "mars::AstronautLocalizerTask"); + + /** Default deconstructor of AstronautLocalizerTask + */ + ~AstronautLocalizerTask(); + + /** This hook is called by Orocos when the state machine transitions + * from PreOperational to Stopped. If it returns false, then the + * component will stay in PreOperational. Otherwise, it goes into + * Stopped. + * + * It is meaningful only if the #needs_configuration has been specified + * in the task context definition with (for example): + \verbatim + task_context "TaskName" do + needs_configuration + ... + end + \endverbatim + */ + bool configureHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Stopped to Running. If it returns false, then the component will + * stay in Stopped. Otherwise, it goes into Running and updateHook() + * will be called. + */ + bool startHook(); + + /** This hook is called by Orocos when the component is in the Running + * state, at each activity step. Here, the activity gives the "ticks" + * when the hook should be called. + * + * The error(), exception() and fatal() calls, when called in this hook, + * allow to get into the associated RunTimeError, Exception and + * FatalError states. + * + * In the first case, updateHook() is still called, and recover() allows + * you to go back into the Running state. In the second case, the + * errorHook() will be called instead of updateHook(). In Exception, the + * component is stopped and recover() needs to be called before starting + * it again. Finally, FatalError cannot be recovered. + */ + void updateHook(); + + /** This hook is called by Orocos when the component is in the + * RunTimeError state, at each activity step. See the discussion in + * updateHook() about triggering options. + * + * Call recover() to go back in the Runtime state. + */ + void errorHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Running to Stopped after stop() has been called. + */ + void stopHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Stopped to PreOperational, requiring the call to configureHook() + * before calling start() again. + */ + void cleanupHook(); + }; +} + +#endif + diff --git a/tasks/CMakeLists.txt b/tasks/CMakeLists.txt index 316428eb..1bb38a6a 100644 --- a/tasks/CMakeLists.txt +++ b/tasks/CMakeLists.txt @@ -1,8 +1,14 @@ # Generated from orogen/lib/orogen/templates/tasks/CMakeLists.txt find_package( Boost COMPONENTS filesystem) +find_package(lib_manager) +setup_qt() + +pkg_check_modules(PKGCONFIG REQUIRED + mars_smurf_loader +) include(marsTaskLib) -ADD_LIBRARY(${MARS_TASKLIB_NAME} SHARED +ADD_LIBRARY(${MARS_TASKLIB_NAME} SHARED ${MARS_TASKLIB_SOURCES}) add_dependencies(${MARS_TASKLIB_NAME} regen-typekit) @@ -10,6 +16,9 @@ add_dependencies(${MARS_TASKLIB_NAME} TARGET_LINK_LIBRARIES(${MARS_TASKLIB_NAME} ${OrocosRTT_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY} + ${QT_LIBRARIES} + ${PKGCONFIG_LIBRARIES} + ${QT_QTXML_LIBRARY} ${MARS_TASKLIB_DEPENDENT_LIBRARIES}) SET_TARGET_PROPERTIES(${MARS_TASKLIB_NAME} PROPERTIES LINK_INTERFACE_LIBRARIES "${MARS_TASKLIB_INTERFACE_LIBRARIES}") diff --git a/tasks/EntityFakeDetection.cpp b/tasks/EntityFakeDetection.cpp index 8ed64c24..2cdfec2e 100644 --- a/tasks/EntityFakeDetection.cpp +++ b/tasks/EntityFakeDetection.cpp @@ -118,6 +118,12 @@ namespace mars { iter->second->getBoundingBox(center, rotation, detectionArray->detections[i].bbox.size); + if (frame_id==FrameId::CAMERA) { + cameraStruct cs; + camera->getCameraInfo(&cs); + center = -(cs.rot.inverse() * cs.pos) + cs.rot.inverse() * center; + rotation = cs.rot.inverse() * rotation; + } detectionArray->detections[i].bbox.center.position = center; detectionArray->detections[i].bbox.center.orientation = rotation; //ObjectHypothesisWithPose @@ -158,4 +164,3 @@ namespace mars { { } } - diff --git a/tasks/EntityFakeDetection.hpp b/tasks/EntityFakeDetection.hpp index 501e9963..a393726e 100644 --- a/tasks/EntityFakeDetection.hpp +++ b/tasks/EntityFakeDetection.hpp @@ -34,7 +34,6 @@ namespace mars { protected: enum FrameId { - NO_FRAME, GLOBAL, CAMERA }; @@ -128,4 +127,3 @@ namespace mars { } #endif - diff --git a/tasks/PoseUpdateTask.cpp b/tasks/PoseUpdateTask.cpp new file mode 100755 index 00000000..acc40697 --- /dev/null +++ b/tasks/PoseUpdateTask.cpp @@ -0,0 +1,154 @@ +/* Generated from orogen/lib/orogen/templates/tasks/IMU.cpp */ + +#include "PoseUpdateTask.hpp" +#include "Plugin.hpp" +#include +#include +#include +#include +#include +#include + +#include + + +namespace mars { + + using namespace interfaces; + using namespace utils; + + + PoseUpdateTask::PoseUpdateTask(std::string const& name) + : PoseUpdateTaskBase(name) + { + } + + PoseUpdateTask::PoseUpdateTask(std::string const& name, RTT::ExecutionEngine* engine) + : PoseUpdateTaskBase(name, engine) + { + } + + PoseUpdateTask::~PoseUpdateTask() + { + } + + void PoseUpdateTask::applyPose(std::string entityname, base::Pose pose) { + sim::SimEntity* ent = control->entities->getEntity(entityname); + + if (ent == nullptr) { + std::cerr << "mars::PoseUpdateTask | given entity name \"" << entityname << "\" could not be found in entity manager." << std::endl; + return; + } + + configmaps::ConfigMap cfg = ent->getConfig(); + cfg["position"][0] = pose.position[0]; + cfg["position"][1] = pose.position[1]; + cfg["position"][2] = pose.position[2]; + + cfg["rotation"][0] = pose.orientation.w(); + cfg["rotation"][1] = pose.orientation.x(); + cfg["rotation"][2] = pose.orientation.y(); + cfg["rotation"][3] = pose.orientation.z(); + + control->sim->physicsThreadLock(); + ent->setInitialPose(true, &cfg); + control->sim->physicsThreadUnlock(); + } + +// void PoseUpdateTask::applyRelativePose(std::string entityname, base::Pose pose) { + +// sim::SimEntity* ent = control->entities->getEntity(entityname); +// if (ent == nullptr) { +// std::cerr << "mars::PoseUpdateTask | given entity name \"" << entityname << "\" could not be found in entity manager." << std::endl; +// return; +// } + +// configmaps::ConfigMap cfg; +// cfg["position"][0] = ent->getPosition() + pose.position[0]; +// cfg["position"][1] = ent->getPosition() + pose.position[1]; +// cfg["position"][2] = ent->getPosition() + pose.position[2]; + +// utils::Quaternion q = ent->getOrientation(); +// utils::Quaternion new_q = q * pose.orientation; + +// cfg["rotation"][0] = new_q.w(); +// cfg["rotation"][1] = new_q.x(); +// cfg["rotation"][2] = new_q.y(); +// cfg["rotation"][3] = new_q.z(); + +// control->sim->physicsThreadLock(); +// ent->setInitialPose(true, &cfg); +// control->sim->physicsThreadUnlock(); +// } + + void PoseUpdateTask::init() + { + } + + void PoseUpdateTask::update(double delta_t) + { + } + + /// The following lines are template definitions for the various state machine + // hooks defined by Orocos::RTT. See PoseUpdateTask.hpp for more detailed + // documentation about them. + + bool PoseUpdateTask::configureHook() + { + if (! PoseUpdateTaskBase::configureHook()) return false; + return true; + } + + bool PoseUpdateTask::startHook() + { + return PoseUpdateTaskBase::startHook(); + } + + void PoseUpdateTask::updateHook() + { + PoseUpdateTaskBase::updateHook(); + + if(!isRunning()) return; //Seems Plugin is set up but not active yet, we are not sure that we are initialized correctly so retuning + + mars::PoseUpdate update; + if (_single_pose_update.readNewest(update)) { + applyPose(update.entity_name, update.pose); + } + + + mars::PoseUpdates updates; + if (_multi_pose_updates.readNewest(updates)) { + for (auto u: updates) { + applyPose(u.entity_name, u.pose); + } + } + + // mars::PoseUpdate rel_update; + // if (_single_relatve_pose_update.readNewest(rel_update)) { + // applyRelativePose(update.entity_name, update.pose); + // } + + + // mars::PoseUpdates rel_updates; + // if (_multi_relatve_pose_updates.readNewest(rel_updates)) { + // for (auto u: updates) { + // applyRelativePose(u.entity_name, u.pose); + // } + // } + } + + // void PoseUpdateTask::errorHook() + // { + // PoseUpdateTaskBase::errorHook(); + // } + + void PoseUpdateTask::stopHook() + { + PoseUpdateTaskBase::stopHook(); + } + + // void PoseUpdateTask::cleanupHook() + // { + // PoseUpdateTaskBase::cleanupHook(); + // } +} diff --git a/tasks/PoseUpdateTask.hpp b/tasks/PoseUpdateTask.hpp new file mode 100644 index 00000000..9c71d584 --- /dev/null +++ b/tasks/PoseUpdateTask.hpp @@ -0,0 +1,117 @@ +#ifndef SIMULATION_POSE_UPDATE_TASK_HPP +#define SIMULATION_POSE_UPDATE_TASK_HPP + +#include "mars/PoseUpdateTaskBase.hpp" +#include +#include +#include +namespace mars { + using namespace interfaces; + + class PoseUpdateTaskPlugin; + + /*! \class PoseUpdateTask + * \brief The task context provides and requires services. It uses an ExecutionEngine to perform its functions. + * Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. + * In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow. + * + * \details + * The name of a TaskContext is primarily defined via: + \verbatim + deployment 'deployment_name' + task('custom_task_name','mars::PoseUpdateTask') + end + \endverbatim + * It can be dynamically adapted when the deployment is called with a prefix argument. + */ + class PoseUpdateTask : public PoseUpdateTaskBase + { + friend class PoseUpdateTaskBase; + + protected: + + public: + virtual void init(); + virtual void update(double delta_t); + /** TaskContext constructor for PoseUpdateTask + * \param name Name of the task. This name needs to be unique to make it identifiable via nameservices. + * \param initial_state The initial TaskState of the TaskContext. Default is Stopped state. + */ + PoseUpdateTask(std::string const& name = "mars::PoseUpdateTask"); + + /** TaskContext constructor for PoseUpdateTask + * \param name Name of the task. This name needs to be unique to make it identifiable for nameservices. + * \param engine The RTT Execution engine to be used for this task, which serialises the execution of all commands, programs, state machines and incoming events for a task. + * + */ + PoseUpdateTask(std::string const& name, RTT::ExecutionEngine* engine); + + /** Default deconstructor of PoseUpdateTask + */ + ~PoseUpdateTask(); + + void applyPose(std::string, base::Pose pose); + //void applyRelativePose(std::string, base::Pose pose); + + /** This hook is called by Orocos when the state machine transitions + * from PreOperational to Stopped. If it returns false, then the + * component will stay in PreOperational. Otherwise, it goes into + * Stopped. + * + * It is meaningful only if the #needs_configuration has been specified + * in the task context definition with (for example): + \verbatim + task_context "TaskName" do + needs_configuration + ... + end + \endverbatim + */ + bool configureHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Stopped to Running. If it returns false, then the component will + * stay in Stopped. Otherwise, it goes into Running and updateHook() + * will be called. + */ + bool startHook(); + + /** This hook is called by Orocos when the component is in the Running + * state, at each activity step. Here, the activity gives the "ticks" + * when the hook should be called. + * + * The error(), exception() and fatal() calls, when called in this hook, + * allow to get into the associated RunTimeError, Exception and + * FatalError states. + * + * In the first case, updateHook() is still called, and recover() allows + * you to go back into the Running state. In the second case, the + * errorHook() will be called instead of updateHook(). In Exception, the + * component is stopped and recover() needs to be called before starting + * it again. Finally, FatalError cannot be recovered. + */ + void updateHook(); + + /** This hook is called by Orocos when the component is in the + * RunTimeError state, at each activity step. See the discussion in + * updateHook() about triggering options. + * + * Call recover() to go back in the Runtime state. + */ + // void errorHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Running to Stopped after stop() has been called. + */ + void stopHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Stopped to PreOperational, requiring the call to configureHook() + * before calling start() again. + */ + // void cleanupHook(); + }; +} + +#endif + diff --git a/tasks/RepositionTask.cpp b/tasks/RepositionTask.cpp new file mode 100755 index 00000000..e69ee663 --- /dev/null +++ b/tasks/RepositionTask.cpp @@ -0,0 +1,153 @@ +/* Generated from orogen/lib/orogen/templates/tasks/IMU.cpp */ + +#include "RepositionTask.hpp" +#include "Plugin.hpp" +#include +#include +#include +#include +#include +#include + +#include + + +namespace mars { + + using namespace interfaces; + using namespace utils; + + + RepositionTask::RepositionTask(std::string const& name) + : RepositionTaskBase(name) + { + } + + RepositionTask::RepositionTask(std::string const& name, RTT::ExecutionEngine* engine) + : RepositionTaskBase(name, engine) + { + } + + RepositionTask::~RepositionTask() + { + } + + void RepositionTask::applyPose(std::string entityname, base::Pose pose) { + configmaps::ConfigMap cfg; + for (auto it: scene) { + if (it["name"] == entityname) { + cfg = it; + break; + } + } + + cfg["position"][0] = pose.position[0]; + cfg["position"][1] = pose.position[1]; + cfg["position"][2] = pose.position[2]; + + cfg["rotation"][0] = pose.orientation.w(); + cfg["rotation"][1] = pose.orientation.x(); + cfg["rotation"][2] = pose.orientation.y(); + cfg["rotation"][3] = pose.orientation.z(); + + sim::SimEntity* ent = control->entities->getEntity(entityname); + control->sim->physicsThreadLock(); + ent->setInitialPose(true, &cfg); + control->sim->physicsThreadUnlock(); + } + + void RepositionTask::init() + { + } + + void RepositionTask::update(double delta_t) + { + } + + /// The following lines are template definitions for the various state machine + // hooks defined by Orocos::RTT. See RepositionTask.hpp for more detailed + // documentation about them. + + bool RepositionTask::configureHook() + { + if (! RepositionTaskBase::configureHook()) return false; + scene_path = _scene_path.get(); + entity_names = _entity_names.get(); + // Parse the cfg and save the relevant parts + configmaps::ConfigMap scene_map = configmaps::ConfigMap::fromYamlFile(scene_path); + LOG_DEBUG("Using scene/assembly %s\n", scene_path.c_str()); + if (scene_map.hasKey("smurfs")) { + scene = scene_map["smurfs"]; + } else if (scene_map.hasKey("smurfa")) { + scene = scene_map["smurfa"]; + } else if (scene_map.hasKey("entities")) { + scene = scene_map["entities"]; + } else { + throw std::invalid_argument ("File content ("+scene_path+") invalid!"); + } + + //find link and parent ids + for (auto ent_it: entity_names) { + sim::SimEntity* e = control->entities->getEntity(ent_it); + if (e) { + // link_ids + link_ids[ent_it] = e->getRootestId(); + // find parent id + for (auto scn_it: scene) { + if (scn_it.hasKey("parent") && ((std::string) scn_it["name"])==ent_it) { + std::string par = scn_it["parent"]; + int pos = par.find("::"); + parent_ids[link_ids[ent_it]] = control->entities->getEntityNode(par.substr(0,pos), par.substr(pos+2)); + children_ids[control->entities->getEntity(par.substr(0,pos))->getRootestId()].push_back(link_ids[ent_it]); + } + } + } else { + throw std::invalid_argument ("No such entity with name " + ent_it + " found!"); + } + } + + return true; + } + + bool RepositionTask::startHook() + { + return RepositionTaskBase::startHook(); + } + + void RepositionTask::updateHook() + { + RepositionTaskBase::updateHook(); + + if(!isRunning()) return; //Seems Plugin is set up but not active yet, we are not sure that we are initialized correctly so retuning + + std::vector poses; + if (_poses.readNewest(poses)) { + assert(poses.size()==6); + for (unsigned int i=0;i<6/*poses.size()*/;i++) { + base::Pose tmp; + if (i +#include +#include +namespace mars { + using namespace interfaces; + + class RepositionTaskPlugin; + + /*! \class RepositionTask + * \brief The task context provides and requires services. It uses an ExecutionEngine to perform its functions. + * Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. + * In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow. + * + * \details + * The name of a TaskContext is primarily defined via: + \verbatim + deployment 'deployment_name' + task('custom_task_name','mars::RepositionTask') + end + \endverbatim + * It can be dynamically adapted when the deployment is called with a prefix argument. + */ + class RepositionTask : public RepositionTaskBase + { + friend class RepositionTaskBase; + + protected: + std::string scene_path; + std::vector entity_names; + std::map link_ids; + /// contains the parent node id of the given link id + std::map parent_ids; + std::map> children_ids; + configmaps::ConfigVector scene; + + public: + virtual void init(); + virtual void update(double delta_t); + /** TaskContext constructor for RepositionTask + * \param name Name of the task. This name needs to be unique to make it identifiable via nameservices. + * \param initial_state The initial TaskState of the TaskContext. Default is Stopped state. + */ + RepositionTask(std::string const& name = "mars::RepositionTask"); + + /** TaskContext constructor for RepositionTask + * \param name Name of the task. This name needs to be unique to make it identifiable for nameservices. + * \param engine The RTT Execution engine to be used for this task, which serialises the execution of all commands, programs, state machines and incoming events for a task. + * + */ + RepositionTask(std::string const& name, RTT::ExecutionEngine* engine); + + /** Default deconstructor of RepositionTask + */ + ~RepositionTask(); + + void applyPose(std::string, base::Pose pose); + + /** This hook is called by Orocos when the state machine transitions + * from PreOperational to Stopped. If it returns false, then the + * component will stay in PreOperational. Otherwise, it goes into + * Stopped. + * + * It is meaningful only if the #needs_configuration has been specified + * in the task context definition with (for example): + \verbatim + task_context "TaskName" do + needs_configuration + ... + end + \endverbatim + */ + bool configureHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Stopped to Running. If it returns false, then the component will + * stay in Stopped. Otherwise, it goes into Running and updateHook() + * will be called. + */ + bool startHook(); + + /** This hook is called by Orocos when the component is in the Running + * state, at each activity step. Here, the activity gives the "ticks" + * when the hook should be called. + * + * The error(), exception() and fatal() calls, when called in this hook, + * allow to get into the associated RunTimeError, Exception and + * FatalError states. + * + * In the first case, updateHook() is still called, and recover() allows + * you to go back into the Running state. In the second case, the + * errorHook() will be called instead of updateHook(). In Exception, the + * component is stopped and recover() needs to be called before starting + * it again. Finally, FatalError cannot be recovered. + */ + void updateHook(); + + /** This hook is called by Orocos when the component is in the + * RunTimeError state, at each activity step. See the discussion in + * updateHook() about triggering options. + * + * Call recover() to go back in the Runtime state. + */ + // void errorHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Running to Stopped after stop() has been called. + */ + void stopHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Stopped to PreOperational, requiring the call to configureHook() + * before calling start() again. + */ + // void cleanupHook(); + }; +} + +#endif + diff --git a/tasks/RobotTeleportation.cpp b/tasks/RobotTeleportation.cpp index d29e1294..65aff071 100644 --- a/tasks/RobotTeleportation.cpp +++ b/tasks/RobotTeleportation.cpp @@ -6,10 +6,12 @@ #include #include #include -//#include -//#include -//#include +#include +#include +#include +#include #include +#include #include @@ -49,47 +51,53 @@ namespace mars { bool RobotTeleportation::configureHook() { if (! RobotTeleportationBase::configureHook()) return false; - //get robot data + scene_path = _scene_path.get(); robot_name = _robot_name.get(); - robot_entity = control->entities->getEntity(robot_name); - LOG_DEBUG_S << "RobotTeleportation: " << "got robot: "<< robot_name; - reset_node_name = _reset_node_name.get(); - LOG_DEBUG_S << "RobotTeleportation: " << "got robot: "<< reset_node_name; - //get config properties pos_mode = _position_mode.get(); - LOG_DEBUG_S << "RobotTeleportation: " << "got config"; - //save initial position + // Parse the cfg and save the relevant parts + configmaps::ConfigMap map = configmaps::ConfigMap::fromYamlFile(scene_path); + configmaps::ConfigVector cfg; + if (map.hasKey("smurfs")) { + cfg = map["smurfs"]; + } else if (map.hasKey("entities")) { + cfg = map["entities"]; + } else { + throw std::invalid_argument ("Scene file invalid!"); + } + + bool found = false; + for (configmaps::ConfigVector::iterator it = cfg.begin(); it!=cfg.end(); it++) { + configmaps::ConfigMap m = *it; + if (m.hasKey("name") && m["name"] == robot_name) { + map = (configmaps::ConfigMap) m; + found = true; + } + } + if (!found) { + throw std::invalid_argument ("Scene file invalid. robot_name not found!"); + } + + if (!map.hasKey("teleportation_targets") && pos_mode != 0) + throw std::invalid_argument ("No targets given!"); target.id = 0; - target.cfg = robot_entity->getConfig(); - //initiate target vector + target.cfg = map; + reloadScene = false; targets.push_back(target); - LOG_DEBUG_S << "RobotTeleportation: " << "got current state"; - //get target positions from file - if (pos_mode == 1) { - //load target config list - ConfigVector::iterator mIt = target.cfg["teleportation_targets"].begin(); - unsigned int id = 1; //first item is initial position - for (; mIt != target.cfg["teleportation_targets"].end(); ++mIt) { - Target t; - t.id = ++id; - t.cfg = target.cfg; - for (FIFOMap::iterator it = mIt->beginMap(); it!=mIt->endMap(); ++it) { - if (it->first == "file") { - unsigned int pos = ((std::string)it->second).find_last_of("/")+1; - if (pos != std::string::npos) { - t.cfg["file"] = ((std::string)it->second).substr(pos); - t.cfg["path"] = (std::string)target.cfg["load_path"]+((std::string)it->second).substr(0,pos); - } else { - t.cfg[it->first] = it->second; - } - } else { - t.cfg[it->first] = it->second; - } - } - targets.push_back(t); + for (auto it: map["teleportation_targets"]) { + //go through the teleportation_targets and save the changed configurations + target.id++; + configmaps::ConfigMap m = it; + for (auto it2 = m.begin(); it2!=m.end(); it2++) { + target.cfg[it2->first] = it2->second; } - LOG_DEBUG_S << "RobotTeleportation: " << "read targets"; + targets.push_back(target); + if (it.hasKey("file") || it.hasKey("path")) reloadScene = true; } + // initialize with target 0 + robot_entity = control->entities->getEntity(robot_name); + LOG_DEBUG_S << "RobotTeleportation: " << "got robot: "<< robot_name; + reset_node_name = _reset_node_name.get(); + LOG_DEBUG_S << "RobotTeleportation: " << "got robot: "<< reset_node_name; LOG_DEBUG_S << "RobotTeleportation: "<< "Task configured! " << (pos_mode ? "(Manual)":"(Preconfigured)"); @@ -104,15 +112,22 @@ namespace mars { void RobotTeleportation::updateHook() { RobotTeleportationBase::updateHook(); - bool reloadScene = false; if(!isRunning()) return; //Seems Plugin is set up but not active yet, we are not sure that we are initialized correctly so retuning //if triggered load new position - if (_position_id.readNewest(curr_id) == RTT::NewData || - _position.readNewest(pos) == RTT::NewData || - _rotation.readNewest(rot) == RTT::NewData || - _anchor.readNewest(anchor) == RTT::NewData || + old_curr_id = curr_id; + old_pos = pos; + old_rot = rot; + old_anchor = anchor; + _position_id.readNewest(curr_id); + _position.readNewest(pos); + _rotation.readNewest(rot); + _anchor.readNewest(anchor); + if (old_curr_id != curr_id|| + old_pos != pos|| + old_rot.x() != rot.x() || old_rot.y() != rot.y() || old_rot.z() != rot.z() || old_rot.w() != rot.w() || + old_anchor != anchor|| _reset_node.readNewest(reset_node) == RTT::NewData ) { @@ -122,9 +137,6 @@ namespace mars { LOG_DEBUG_S << "RobotTeleportation: " << "given to high id " << curr_id << ">=" <sim->physicsThreadLock(); LOG_DEBUG_S << "RobotTeleportation: Reloading the following cfg:\n" << target.cfg.toYamlString(); - control->entities->removeEntity(robot_name); - control->sim->loadScene((std::string)target.cfg["path"]+(std::string)target.cfg["file"], robot_name); + control->entities->removeEntity(robot_name, true /*remove the complete assembly*/); + dynamic_cast(control->loadCenter->loadScene[".smurfs"])->loadEntity(&target.cfg, getPathOfFile(scene_path)); robot_entity = control->entities->getEntity(robot_name); + control->sim->physicsThreadUnlock(); + } else { + robot_entity->setInitialPose(robot_entity->hasAnchorJoint(), &target.cfg); } - robot_entity->setInitialPose(robot_entity->hasAnchorJoint(), &target.cfg); - /* if the robot has suspension joints we have to teleport the root node to the new position as well*/ NodeId id = control->nodes->getID(reset_node_name); - if (id == INVALID_ID) LOG_DEBUG_S << "RobotTeleportation: "<< "Reset node not found: "<< reset_node_name <<"!"; + if (id == INVALID_ID) {LOG_DEBUG_S << "RobotTeleportation: "<< "Reset node not found: "<< reset_node_name <<"!";} if (target.cfg.find("reset_node") != target.cfg.end() && target.cfg["reset_node"] && id != INVALID_ID) { LOG_DEBUG_S << "RobotTeleportation: "<< "Resetting node "<< reset_node_name <<"!"; Quaternion tmpQ(1, 0, 0, 0); diff --git a/tasks/RobotTeleportation.hpp b/tasks/RobotTeleportation.hpp index 434506cb..f60d1517 100644 --- a/tasks/RobotTeleportation.hpp +++ b/tasks/RobotTeleportation.hpp @@ -33,7 +33,7 @@ namespace mars { friend class RobotTeleportationBase; protected: - std::string robot_name, reset_node_name; + std::string robot_name, reset_node_name, scene_path; sim::SimEntity* robot_entity; unsigned int pos_mode; configmaps::ConfigMap cmap; @@ -41,10 +41,11 @@ namespace mars { JointId joint_id; //input - short unsigned int curr_id; - utils::Vector pos; - utils::Quaternion rot; - bool anchor, reset_node; + short unsigned int curr_id, old_curr_id; + utils::Vector pos, old_pos; + utils::Quaternion rot, old_rot; + bool anchor, reset_node, old_anchor; + bool reloadScene; struct Target {