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;
|
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) {
|
||||||
|
|||||||
@ -33,7 +33,7 @@
|
|||||||
*********************************************************************/
|
*********************************************************************/
|
||||||
|
|
||||||
/* Author: Robert Haschke
|
/* Author: Robert Haschke
|
||||||
Desc: Monitor manipulation tasks and visualize their solutions
|
Desc: Monitor manipulation tasks and visualize their solutions
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "task_display.h"
|
#include "task_display.h"
|
||||||
@ -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);
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user