From 0a299b380ecc57c77b4860cbf87a8166601a2e8d Mon Sep 17 00:00:00 2001 From: Henning Wiedemann Date: Mon, 28 Jan 2019 09:57:31 +0100 Subject: [PATCH 01/11] [RobotTeleportation] Implement switching of smurfa files --- tasks/RobotTeleportation.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/tasks/RobotTeleportation.cpp b/tasks/RobotTeleportation.cpp index d29e1294..a3c8bedf 100644 --- a/tasks/RobotTeleportation.cpp +++ b/tasks/RobotTeleportation.cpp @@ -117,14 +117,21 @@ namespace mars { ) { LOG_DEBUG_S << "RobotTeleportation: " << "updateHook triggered!"; + bool is_smurfa = false; if (pos_mode == 1){ if (curr_id >=targets.size()) { LOG_DEBUG_S << "RobotTeleportation: " << "given to high id " << curr_id << ">=" <entities->removeEntity(robot_name); + if (target.cfg.hasKey("type") && target.cfg["type"] == "smurfa" || is_smurfa) { + control->entities->removeAssembly(robot_name); + } else { + control->entities->removeEntity(robot_name); + } control->sim->loadScene((std::string)target.cfg["path"]+(std::string)target.cfg["file"], robot_name); robot_entity = control->entities->getEntity(robot_name); } From bf63cabd7f3d761a569ba6dac747e18aa72266f5 Mon Sep 17 00:00:00 2001 From: Henning Wiedemann Date: Mon, 28 Jan 2019 17:50:54 +0100 Subject: [PATCH 02/11] [RobotTeleportation] Save config from scene file and reload scene if required (smurfa teleportation possible) --- mars.orogen | 2 + tasks/CMakeLists.txt | 11 ++- tasks/RobotTeleportation.cpp | 128 ++++++++++++++++++----------------- tasks/RobotTeleportation.hpp | 11 +-- 4 files changed, 84 insertions(+), 68 deletions(-) diff --git a/mars.orogen b/mars.orogen index e81c6bf4..5989f0b3 100644 --- a/mars.orogen +++ b/mars.orogen @@ -355,6 +355,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). 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/RobotTeleportation.cpp b/tasks/RobotTeleportation.cpp index a3c8bedf..4e971ff5 100644 --- a/tasks/RobotTeleportation.cpp +++ b/tasks/RobotTeleportation.cpp @@ -5,11 +5,14 @@ #include #include #include +#include #include -//#include -//#include -//#include +#include +#include +#include +#include #include +#include #include @@ -49,47 +52,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,34 +113,31 @@ 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 ) { LOG_DEBUG_S << "RobotTeleportation: " << "updateHook triggered!"; - bool is_smurfa = false; if (pos_mode == 1){ if (curr_id >=targets.size()) { LOG_DEBUG_S << "RobotTeleportation: " << "given to high id " << curr_id << ">=" <sim->physicsThreadLock(); LOG_DEBUG_S << "RobotTeleportation: Reloading the following cfg:\n" << target.cfg.toYamlString(); - if (target.cfg.hasKey("type") && target.cfg["type"] == "smurfa" || is_smurfa) { - control->entities->removeAssembly(robot_name); - } else { - 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 { From feb4ddcba2d161cc795897213100798ec724497c Mon Sep 17 00:00:00 2001 From: Henning Wiedemann Date: Mon, 6 May 2019 15:16:02 +0200 Subject: [PATCH 03/11] Add RepositionTask --- mars.orogen | 17 +++++ tasks/RepositionTask.cpp | 161 +++++++++++++++++++++++++++++++++++++++ tasks/RepositionTask.hpp | 125 ++++++++++++++++++++++++++++++ 3 files changed, 303 insertions(+) create mode 100644 tasks/RepositionTask.cpp create mode 100644 tasks/RepositionTask.hpp diff --git a/mars.orogen b/mars.orogen index 5989f0b3..e63c1da1 100644 --- a/mars.orogen +++ b/mars.orogen @@ -378,6 +378,23 @@ task_context "RobotTeleportation" do 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)") + + input_port('poses', '/std/vector'). + doc 'Poses relative to the parent link' + port_driven + + needs_configuration + +end + task_context "ObjectHighlighter" do subclasses "Plugin" diff --git a/tasks/RepositionTask.cpp b/tasks/RepositionTask.cpp new file mode 100644 index 00000000..aef7e913 --- /dev/null +++ b/tasks/RepositionTask.cpp @@ -0,0 +1,161 @@ +/* Generated from orogen/lib/orogen/templates/tasks/IMU.cpp */ +#define TEST 0 +#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; + _poses.readNewest(poses); + for (unsigned int i=0;i<6/*poses.size()*/;i++) { + // get current position + // #if TEST + // applyPose(link_ids[entity_names[i]], poses[i]); + // #else + 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 + From 191783e09f933e7b30cdfd8e0018b6a90c6f0006 Mon Sep 17 00:00:00 2001 From: Henning Wiedemann Date: Mon, 6 May 2019 15:30:02 +0200 Subject: [PATCH 04/11] Delete unnecessary debug variable --- tasks/RepositionTask.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tasks/RepositionTask.cpp b/tasks/RepositionTask.cpp index aef7e913..d1446b38 100644 --- a/tasks/RepositionTask.cpp +++ b/tasks/RepositionTask.cpp @@ -1,5 +1,5 @@ /* Generated from orogen/lib/orogen/templates/tasks/IMU.cpp */ -#define TEST 0 + #include "RepositionTask.hpp" #include "Plugin.hpp" #include From 0c5aa985024b2bce12be1040084168b66c76f2a3 Mon Sep 17 00:00:00 2001 From: Henning Wiedemann Date: Mon, 6 May 2019 15:40:16 +0200 Subject: [PATCH 05/11] Remove duplicate include --- tasks/RobotTeleportation.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tasks/RobotTeleportation.cpp b/tasks/RobotTeleportation.cpp index 4e971ff5..65aff071 100644 --- a/tasks/RobotTeleportation.cpp +++ b/tasks/RobotTeleportation.cpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include #include From 504930b3ea080ff81728f12fa2cb8753963caf85 Mon Sep 17 00:00:00 2001 From: Henning Wiedemann Date: Mon, 20 May 2019 14:57:33 +0200 Subject: [PATCH 06/11] [RepositionTask] Add ignore_position input, clean up --- mars.orogen | 2 ++ tasks/RepositionTask.cpp | 32 ++++++++++++-------------------- 2 files changed, 14 insertions(+), 20 deletions(-) mode change 100644 => 100755 mars.orogen mode change 100644 => 100755 tasks/RepositionTask.cpp diff --git a/mars.orogen b/mars.orogen old mode 100644 new mode 100755 index 09f1af5c..24089e27 --- a/mars.orogen +++ b/mars.orogen @@ -395,6 +395,8 @@ task_context "RepositionTask" do 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' diff --git a/tasks/RepositionTask.cpp b/tasks/RepositionTask.cpp old mode 100644 new mode 100755 index d1446b38..e69ee663 --- a/tasks/RepositionTask.cpp +++ b/tasks/RepositionTask.cpp @@ -121,26 +121,19 @@ namespace mars { 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; - _poses.readNewest(poses); - for (unsigned int i=0;i<6/*poses.size()*/;i++) { - // get current position - // #if TEST - // applyPose(link_ids[entity_names[i]], poses[i]); - // #else - base::Pose tmp; - if (i Date: Tue, 10 Sep 2019 10:10:05 +0200 Subject: [PATCH 07/11] Fix ObjectDetection in Camera Frame --- tasks/EntityFakeDetection.cpp | 7 ++++++- tasks/EntityFakeDetection.hpp | 2 -- 2 files changed, 6 insertions(+), 3 deletions(-) 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 - From 46045e287455155435993a13fbecb743c9fb0b15 Mon Sep 17 00:00:00 2001 From: Michael Maurus Date: Wed, 4 Sep 2019 16:33:02 +0200 Subject: [PATCH 08/11] added Pose Updater Task for setting poses of entities --- mars.orogen | 15 ++++ poseUpdateType.hpp | 20 +++++ tasks/PoseUpdateTask.cpp | 154 +++++++++++++++++++++++++++++++++++++++ tasks/PoseUpdateTask.hpp | 117 +++++++++++++++++++++++++++++ 4 files changed, 306 insertions(+) create mode 100644 poseUpdateType.hpp create mode 100755 tasks/PoseUpdateTask.cpp create mode 100644 tasks/PoseUpdateTask.hpp diff --git a/mars.orogen b/mars.orogen index ba964bdd..e86c2003 100755 --- 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" @@ -406,6 +407,20 @@ task_context "RepositionTask" do 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/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/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 + From 83ef464a1e1c36537f0d3e00daa83c6cc6e78bbb Mon Sep 17 00:00:00 2001 From: Michael Maurus Date: Tue, 10 Sep 2019 10:14:38 +0200 Subject: [PATCH 09/11] added astronaut lokalizer task --- mars.orogen | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/mars.orogen b/mars.orogen index e86c2003..bc60db21 100755 --- a/mars.orogen +++ b/mars.orogen @@ -351,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.' @@ -388,6 +388,16 @@ task_context "RobotTeleportation" do end +task_context "AstronautLokalizerTask" 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" From 6e18b7117ab5416fa233a0c25c577ad8bb4cbddb Mon Sep 17 00:00:00 2001 From: Michael Maurus Date: Tue, 10 Sep 2019 10:35:48 +0200 Subject: [PATCH 10/11] added AstronautLocalizerTask --- mars.orogen | 2 +- tasks/AstronautLocalizerTask.cpp | 70 +++++++++++++++++++++ tasks/AstronautLocalizerTask.hpp | 103 +++++++++++++++++++++++++++++++ 3 files changed, 174 insertions(+), 1 deletion(-) create mode 100644 tasks/AstronautLocalizerTask.cpp create mode 100644 tasks/AstronautLocalizerTask.hpp diff --git a/mars.orogen b/mars.orogen index bc60db21..4f935c86 100755 --- a/mars.orogen +++ b/mars.orogen @@ -388,7 +388,7 @@ task_context "RobotTeleportation" do end -task_context "AstronautLokalizerTask" do +task_context "AstronautLocalizerTask" do subclasses "Plugin" property("entity_name", "/std/string", "astronaut"). 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 + From 878dfe1e47c8d1b8a4cb9d2c77616bda7c8c6a08 Mon Sep 17 00:00:00 2001 From: Nils Niemann Date: Tue, 19 Mar 2019 10:53:43 +0100 Subject: [PATCH 11/11] adds overloads to aggregator::determineTimestamp For the stream aligner and transformer to work, access to a timestamp in the message is needed. By default a member variable called "time" is assumed to exist, which is not the case in the objectDetectionMessages. Therefore, overloads to determineTimestamp are provided which correctly access the timestamp in the header of the messages. --- objectDetectionTypes.hpp | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) 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