From ceaf896bf8a48207f3a628df00f46ffee5f11e60 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 27 Feb 2018 21:45:19 +0100 Subject: [PATCH] position scene node w.r.t. fixed frame --- core/src/marker_tools.cpp | 2 +- .../src/task_display.cpp | 25 ++++++++++++++++++- .../motion_planning_tasks/src/task_display.h | 11 +++++--- 3 files changed, 32 insertions(+), 6 deletions(-) diff --git a/core/src/marker_tools.cpp b/core/src/marker_tools.cpp index 5d43faee..ab62e915 100644 --- a/core/src/marker_tools.cpp +++ b/core/src/marker_tools.cpp @@ -92,7 +92,7 @@ void generateMarkers(const moveit::core::RobotState &robot_state, if (!model) return; visualization_msgs::Marker m; - m.header.frame_id = model->getRoot()->name; + m.header.frame_id = robot_state.getRobotModel()->getModelFrame(); // code adapted from rviz::RobotLink::createVisual() / createCollision() for (const auto &name : *names) { diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index ced3f388..1c2e5ecb 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -33,7 +33,7 @@ *********************************************************************/ /* Author: Robert Haschke - Desc: Monitor manipulation tasks and visualize their solutions + Desc: Monitor manipulation tasks and visualize their solutions */ #include "task_display.h" @@ -53,6 +53,8 @@ #include #include #include +#include +#include #include namespace moveit_rviz_plugin @@ -148,6 +150,7 @@ void TaskDisplay::onEnable() { Display::onEnable(); loadRobotModel(); + calculateOffsetPosition(); } void TaskDisplay::onDisable() @@ -156,6 +159,26 @@ void TaskDisplay::onDisable() trajectory_visual_->onDisable(); } +void TaskDisplay::fixedFrameChanged() +{ + Display::fixedFrameChanged(); + calculateOffsetPosition(); +} + +void TaskDisplay::calculateOffsetPosition() +{ + if (!robot_model_) + return; + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + + context_->getFrameManager()->getTransform(robot_model_->getModelFrame(), ros::Time(0), position, orientation); + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); +} + void TaskDisplay::update(float wall_dt, float ros_dt) { Display::update(wall_dt, ros_dt); diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index d9d34ffa..b6c9c0b3 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -80,10 +80,6 @@ public: virtual void update(float wall_dt, float ros_dt); virtual void reset(); - // overrides from Display - virtual void onInitialize(); - virtual void onEnable(); - virtual void onDisable(); void setName(const QString& name); void setSolutionStatus(bool ok); @@ -92,6 +88,13 @@ public: void clearMarkers(); void showMarkers(const DisplaySolutionPtr &s); +protected: + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void fixedFrameChanged() override; + void calculateOffsetPosition(); + private Q_SLOTS: /** * \brief Slot Event Functions