adds overloads to aggregator::determineTimestamp

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<std/string>").
+        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<base/Pose>').
+        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"

objectDetectionTypes.hpp

@@ -7,6 +7,7 @@
 #include <base/Time.hpp>
 // TODO 1 #include <mars/interfaces/snmesh.h>
 
+
 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

poseUpdateType.hpp

@@ -0,0 +1,20 @@
+#ifndef MARS_POSITION_UPDATE_TYPE_HH
+#define MARS_POSITION_UPDATE_TYPE_HH
+
+#include <vector>
+#include <string>
+
+#include <base/Pose.hpp>
+
+namespace mars 
+{
+
+struct PoseUpdate {
+	std::string entity_name;
+	base::Pose pose;
+};
+
+typedef std::vector<PoseUpdate> PoseUpdates;
+
+}
+#endif

tasks/AstronautLocalizerTask.cpp

@@ -0,0 +1,70 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */
+
+#include "AstronautLocalizerTask.hpp"
+#include <mars/interfaces/sim/EntityManagerInterface.h>
+#include <mars/interfaces/sim/NodeManagerInterface.h>
+#include <mars/interfaces/sim/ControlCenter.h>
+#include <mars/utils/misc.h>
+#include <mars/sim/SimEntity.h>
+
+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();
+}

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
+

tasks/CMakeLists.txt

@@ -1,15 +1,24 @@
 # 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)
 
 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}")

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 {
   {
   }
 }
-

tasks/EntityFakeDetection.hpp

@@ -34,7 +34,6 @@ namespace mars {
 
     protected:
       enum FrameId {
-	      NO_FRAME,
         GLOBAL,
         CAMERA
       };
@@ -128,4 +127,3 @@ namespace mars {
 }
 
 #endif
-