33 #include <boost/bind.hpp> 35 #include <OgreBillboardSet.h> 36 #include <OgreManualObject.h> 37 #include <OgreMatrix4.h> 38 #include <OgreSceneManager.h> 39 #include <OgreSceneNode.h> 50 #include <trajectory_tracker_msgs/PathWithVelocity.h> 51 #include <trajectory_tracker_msgs/PoseStampedWithVelocity.h> 56 #ifdef HAVE_VALIDATE_QUATERNION_H 65 "Line Style",
"Lines",
66 "The rendering operation to use to draw the grid lines.",
74 "The width, in meters, of each path line." 75 "Only works with the 'Billboards' style.",
81 "Color", QColor(25, 255, 0),
82 "Color to draw the path.",
this);
86 "Amount of transparency to apply to the path.",
this);
90 "Number of paths to display.",
95 "Offset", Ogre::Vector3::ZERO,
96 "Allows you to offset the path from the origin of the reference frame. In meters.",
100 "Pose Style",
"None",
"Shape to display the pose as.",
108 "Length of the axes.",
112 "Radius of the axes.",
117 QColor(255, 85, 255),
118 "Color to draw the poses.",
122 "Length of the arrow shaft.",
127 "Length of the arrow head.",
131 "Shaft Diameter", 0.1,
132 "Diameter of the arrow shaft.",
136 "Head Diameter", 0.3,
137 "Diameter of the arrow head.",
170 if (num > axes_vect.size())
172 for (
size_t i = axes_vect.size(); i < num; i++)
177 axes_vect.push_back(axes);
180 else if (num < axes_vect.size())
182 for (
int i = static_cast<int>(axes_vect.size()) - 1;
static_cast<int>(num) <= i; i--)
186 axes_vect.resize(num);
192 if (num > arrow_vect.size())
194 for (
size_t i = arrow_vect.size(); i < num; i++)
197 arrow_vect.push_back(arrow);
200 else if (num < arrow_vect.size())
202 for (
int i = static_cast<int>(arrow_vect.size()) - 1;
static_cast<int>(num) <= i; i--)
204 delete arrow_vect[i];
206 arrow_vect.resize(num);
309 std::vector<rviz::Axes*>& axes_vect =
axes_chain_[i];
310 for (
size_t j = 0; j < axes_vect.size(); j++)
325 std::vector<rviz::Arrow*>& arrow_vect =
arrow_chain_[i];
326 for (
size_t j = 0; j < arrow_vect.size(); j++)
328 arrow_vect[j]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f);
338 std::vector<rviz::Arrow*>& arrow_vect =
arrow_chain_[i];
339 for (
size_t j = 0; j < arrow_vect.size(); j++)
358 manual_object->clear();
360 manual_object =
NULL;
370 delete billboard_line;
371 billboard_line =
NULL;
396 Ogre::ManualObject* manual_object =
scene_manager_->createManualObject();
397 manual_object->setDynamic(
true);
423 Ogre::ManualObject* manual_object =
NULL;
431 manual_object->clear();
436 billboard_line->
clear();
447 #ifdef HAVE_VALIDATE_QUATERNION_H 448 if (!trajectory_tracker_rviz_plugins::validateQuaternions(msg->poses))
451 "quaternions",
"Path '%s' contains unnormalized quaternions. " 452 "This warning will only be output once but may be true for others; " 453 "enable DEBUG messages for ros.rviz.quaternions to see more details.",
456 "quaternions",
"Path '%s' contains unnormalized quaternions.",
462 Ogre::Vector3 position;
463 Ogre::Quaternion orientation;
467 "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable(
fixed_frame_));
470 Ogre::Matrix4 transform(orientation);
471 transform.setTrans(position);
479 uint32_t num_points = msg->poses.size();
485 manual_object->estimateVertexCount(num_points);
486 manual_object->begin(
"BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
487 for (uint32_t i = 0; i < num_points; ++i)
489 const geometry_msgs::Point& pos = msg->poses[i].pose.position;
490 Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
491 manual_object->position(xpos.x, xpos.y, xpos.z);
492 manual_object->colour(color);
495 manual_object->end();
503 for (uint32_t i = 0; i < num_points; ++i)
505 const geometry_msgs::Point& pos = msg->poses[i].pose.position;
506 Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
507 billboard_line->
addPoint(xpos, color);
515 std::vector<rviz::Arrow*>& arrow_vect =
arrow_chain_[bufferIndex];
516 std::vector<rviz::Axes*>& axes_vect =
axes_chain_[bufferIndex];
522 for (uint32_t i = 0; i < num_points; ++i)
524 const geometry_msgs::Point& pos = msg->poses[i].pose.position;
525 Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
526 axes_vect[i]->setPosition(xpos);
527 Ogre::Quaternion orientation(msg->poses[i].pose.orientation.w,
528 msg->poses[i].pose.orientation.x,
529 msg->poses[i].pose.orientation.y,
530 msg->poses[i].pose.orientation.z);
531 axes_vect[i]->setOrientation(orientation);
537 for (uint32_t i = 0; i < num_points; ++i)
539 const geometry_msgs::Point& pos = msg->poses[i].pose.position;
540 Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
543 arrow_vect[i]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f);
549 arrow_vect[i]->setPosition(xpos);
550 Ogre::Quaternion orientation(msg->poses[i].pose.orientation.w,
551 msg->poses[i].pose.orientation.x,
552 msg->poses[i].pose.orientation.y,
553 msg->poses[i].pose.orientation.z);
555 Ogre::Vector3 dir(1, 0, 0);
556 dir = orientation * dir;
557 arrow_vect[i]->setDirection(dir);
virtual QColor getColor() const
rviz::FloatProperty * pose_arrow_shaft_diameter_property_
void updatePoseArrowColor()
void updateBufferLength()
void destroyPoseArrowChain()
rviz::VectorProperty * offset_property_
virtual void onInitialize()
Overridden from Display.
PathWithVelocityDisplay()
Ogre::ColourValue getOgreColor() const
void addPoint(const Ogre::Vector3 &point)
virtual void onInitialize()
rviz::FloatProperty * pose_arrow_head_diameter_property_
DisplayContext * context_
bool validateFloats(const trajectory_tracker_msgs::PoseStampedWithVelocity &msg)
virtual int getInt() const
virtual float getFloat() const
Displays a trajectory_tracker_msgs::PathWithVelocity message.
void destroyPoseAxesChain()
std::vector< ArrowPtrArray > arrow_chain_
Ogre::SceneNode * scene_node_
rviz::FloatProperty * pose_arrow_head_length_property_
rviz::FloatProperty * pose_axes_length_property_
std::vector< AxesPtrArray > axes_chain_
#define ROS_DEBUG_NAMED(name,...)
rviz::ColorProperty * color_property_
void processMessage(const trajectory_tracker_msgs::PathWithVelocity::ConstPtr &msg)
Overridden from MessageFilterDisplay.
rviz::IntProperty * buffer_length_property_
virtual ~PathWithVelocityDisplay()
virtual void addOption(const QString &option, int value=0)
std::vector< rviz::BillboardLine * > billboard_lines_
virtual FrameManager * getFrameManager() const =0
std::vector< Ogre::ManualObject * > manual_objects_
rviz::ColorProperty * pose_arrow_color_property_
void allocateAxesVector(std::vector< rviz::Axes * > &axes_vect, size_t num)
void updatePoseAxisGeometry()
virtual void reset()
Overridden from Display.
uint32_t messages_received_
void setNumLines(uint32_t num)
#define ROS_WARN_ONCE_NAMED(name,...)
void setMaxPointsPerLine(uint32_t max)
rviz::FloatProperty * alpha_property_
Ogre::SceneManager * scene_manager_
virtual void queueRender()=0
virtual Ogre::Vector3 getVector() const
rviz::FloatProperty * line_width_property_
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
void allocateArrowVector(std::vector< rviz::Arrow * > &arrow_vect, size_t num)
rviz::EnumProperty * pose_style_property_
rviz::FloatProperty * pose_arrow_shaft_length_property_
rviz::FloatProperty * pose_axes_radius_property_
void updatePoseArrowGeometry()
rviz::EnumProperty * style_property_
virtual int getOptionInt()
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
virtual QString getName() const
void setLineWidth(float width)