mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
position scene node w.r.t. fixed frame
This commit is contained in:
parent
0494f1c93b
commit
ceaf896bf8
@ -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) {
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user