position scene node w.r.t. fixed frame

This commit is contained in:
Robert Haschke 2018-02-27 21:45:19 +01:00
parent 0494f1c93b
commit ceaf896bf8
3 changed files with 32 additions and 6 deletions

View File

@ -92,7 +92,7 @@ void generateMarkers(const moveit::core::RobotState &robot_state,
if (!model) return; if (!model) return;
visualization_msgs::Marker m; 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() // code adapted from rviz::RobotLink::createVisual() / createCollision()
for (const auto &name : *names) { for (const auto &name : *names) {

View File

@ -53,6 +53,8 @@
#include <rviz/properties/string_property.h> #include <rviz/properties/string_property.h>
#include <rviz/properties/ros_topic_property.h> #include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/status_property.h> #include <rviz/properties/status_property.h>
#include <rviz/frame_manager.h>
#include <OgreSceneNode.h>
#include <QTimer> #include <QTimer>
namespace moveit_rviz_plugin namespace moveit_rviz_plugin
@ -148,6 +150,7 @@ void TaskDisplay::onEnable()
{ {
Display::onEnable(); Display::onEnable();
loadRobotModel(); loadRobotModel();
calculateOffsetPosition();
} }
void TaskDisplay::onDisable() void TaskDisplay::onDisable()
@ -156,6 +159,26 @@ void TaskDisplay::onDisable()
trajectory_visual_->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) void TaskDisplay::update(float wall_dt, float ros_dt)
{ {
Display::update(wall_dt, ros_dt); Display::update(wall_dt, ros_dt);

View File

@ -80,10 +80,6 @@ public:
virtual void update(float wall_dt, float ros_dt); virtual void update(float wall_dt, float ros_dt);
virtual void reset(); virtual void reset();
// overrides from Display
virtual void onInitialize();
virtual void onEnable();
virtual void onDisable();
void setName(const QString& name); void setName(const QString& name);
void setSolutionStatus(bool ok); void setSolutionStatus(bool ok);
@ -92,6 +88,13 @@ public:
void clearMarkers(); void clearMarkers();
void showMarkers(const DisplaySolutionPtr &s); void showMarkers(const DisplaySolutionPtr &s);
protected:
void onInitialize() override;
void onEnable() override;
void onDisable() override;
void fixedFrameChanged() override;
void calculateOffsetPosition();
private Q_SLOTS: private Q_SLOTS:
/** /**
* \brief Slot Event Functions * \brief Slot Event Functions