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;
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) {

View File

@ -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 <rviz/properties/string_property.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/status_property.h>
#include <rviz/frame_manager.h>
#include <OgreSceneNode.h>
#include <QTimer>
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);

View File

@ -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