diff --git a/CMakeLists.txt b/CMakeLists.txt index 038a43136c..0088cc8008 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,6 +137,7 @@ find_package(catkin REQUIRED std_srvs tf2_ros tf2_geometry_msgs + tf2_eigen urdf visualization_msgs ) diff --git a/package.xml b/package.xml index 34e60f544b..a20d45e4ec 100644 --- a/package.xml +++ b/package.xml @@ -53,6 +53,7 @@ std_srvs tf2_ros tf2_geometry_msgs + tf2_eigen tinyxml2 urdf visualization_msgs diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp index 6fb6fac3e9..f339326b2f 100644 --- a/src/rviz/default_plugin/odometry_display.cpp +++ b/src/rviz/default_plugin/odometry_display.cpp @@ -27,12 +27,15 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include + #include #include #include #include #include #include +#include #include #include @@ -47,6 +50,12 @@ namespace rviz { OdometryDisplay::OdometryDisplay() { + continuous_transform_property_ = + new BoolProperty("Continuous Transform", false, + "Retransform into fixed frame every timestep. This is particularly useful for " + "messages whose frame moves w.r.t. fixed frame.", + this); + position_tolerance_property_ = new FloatProperty("Position Tolerance", .1, "Distance, in meters from the last arrow dropped, " "that will cause a new arrow to drop.", @@ -153,6 +162,10 @@ void OdometryDisplay::clear() { last_used_message_.reset(); } + if (ref_pose_) + { + ref_pose_.reset(); + } } void OdometryDisplay::updateColorAndAlpha() @@ -278,21 +291,16 @@ void OdometryDisplay::processMessage(const nav_msgs::Odometry::ConstPtr& message if (last_used_message_) { - Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, - last_used_message_->pose.pose.position.y, - last_used_message_->pose.pose.position.z); - Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, - message->pose.pose.position.z); - Eigen::Quaternionf last_orientation(last_used_message_->pose.pose.orientation.w, - last_used_message_->pose.pose.orientation.x, - last_used_message_->pose.pose.orientation.y, - last_used_message_->pose.pose.orientation.z); - Eigen::Quaternionf current_orientation(message->pose.pose.orientation.w, - message->pose.pose.orientation.x, - message->pose.pose.orientation.y, - message->pose.pose.orientation.z); - - if ((last_position - current_position).length() < position_tolerance_property_->getFloat() && + // use double precision in case the positions are very large, like for example with UTM coords + Eigen::Vector3d last_position, current_position; + Eigen::Quaterniond last_orientation, current_orientation; + + tf2::fromMsg(message->pose.pose.position, current_position); + tf2::fromMsg(message->pose.pose.orientation, current_orientation); + tf2::fromMsg(last_used_message_->pose.pose.position, last_position); + tf2::fromMsg(last_used_message_->pose.pose.orientation, last_orientation); + + if ((last_position - current_position).norm() < position_tolerance_property_->getFloat() && last_orientation.angularDistance(current_orientation) < angle_tolerance_property_->getFloat()) { return; @@ -301,14 +309,37 @@ void OdometryDisplay::processMessage(const nav_msgs::Odometry::ConstPtr& message Ogre::Vector3 position; Ogre::Quaternion orientation; - if (!context_->getFrameManager()->transform(message->header, message->pose.pose, position, orientation)) + if (!continuous_transform_property_->getBool()) { - ROS_ERROR("Error transforming odometry '%s' from frame '%s' to frame '%s'", qPrintable(getName()), - message->header.frame_id.c_str(), qPrintable(fixed_frame_)); - return; + if (!context_->getFrameManager()->transform(message->header, message->pose.pose, position, + orientation)) + { + ROS_ERROR("Error transforming odometry '%s' from frame '%s' to frame '%s'", qPrintable(getName()), + message->header.frame_id.c_str(), qPrintable(fixed_frame_)); + return; + } + + // If we arrive here, we're good. Continue... } + else + { + // put the visuals at the pose specified by the message and transform their scene node continuously - // If we arrive here, we're good. Continue... + // Use ref pose for very large transforms where float (default in OGRE) has not enough precision. + // Such large transforms can occur when converting GPS coordinates to a euclidean coordinate system, + // e.g. UTM. The ref pose is set to the position of the first incoming odometry message and is axis + // aligned to the parent frame of the odometry message. + if (!ref_pose_) + { + ref_pose_.reset(new geometry_msgs::Pose()); + ref_pose_->position = message->pose.pose.position; + } + + const geometry_msgs::Pose& p = message->pose.pose; + position = Ogre::Vector3(p.position.x - ref_pose_->position.x, p.position.y - ref_pose_->position.y, + p.position.z - ref_pose_->position.z); + orientation = Ogre::Quaternion(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z); + } // Create a scene node, and attach the arrow and the covariance to it Axes* axes = new Axes(scene_manager_, scene_node_, axes_length_property_->getFloat(), @@ -372,6 +403,22 @@ void OdometryDisplay::update(float /*wall_dt*/, float /*ros_dt*/) assert(arrows_.size() == axes_.size()); assert(axes_.size() == covariance_property_->sizeVisual()); + + // continuously set the scene node's position to ref pose + if (!continuous_transform_property_->getBool() || !last_used_message_ || !ref_pose_) + return; + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->transform(last_used_message_->header.frame_id, ros::Time(), + *ref_pose_, position, orientation)) + { + // the error output with setStatus is already generated by MessageFilterDisplay + return; + } + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); } void OdometryDisplay::reset() diff --git a/src/rviz/default_plugin/odometry_display.h b/src/rviz/default_plugin/odometry_display.h index e1bf86f98f..731b2130af 100644 --- a/src/rviz/default_plugin/odometry_display.h +++ b/src/rviz/default_plugin/odometry_display.h @@ -51,6 +51,7 @@ class ColorProperty; class FloatProperty; class IntProperty; class EnumProperty; +class BoolProperty; class CovarianceProperty; @@ -102,6 +103,9 @@ private Q_SLOTS: D_Axes axes_; nav_msgs::Odometry::ConstPtr last_used_message_; + geometry_msgs::Pose::Ptr ref_pose_; + + rviz::BoolProperty* continuous_transform_property_; rviz::EnumProperty* shape_property_; diff --git a/src/rviz/default_plugin/point_cloud_common.cpp b/src/rviz/default_plugin/point_cloud_common.cpp index 763b657f70..deca035cd3 100644 --- a/src/rviz/default_plugin/point_cloud_common.cpp +++ b/src/rviz/default_plugin/point_cloud_common.cpp @@ -313,6 +313,12 @@ PointCloudCommon::PointCloudCommon(Display* display) , transformer_class_loader_(nullptr) , display_(display) { + continuous_transform_property_ = + new BoolProperty("Continuous Transform", false, + "Retransform into fixed frame every timestep. This is particularly useful for " + "messages whose frame moves w.r.t. fixed frame.", + display_); + selectable_property_ = new BoolProperty("Selectable", true, "Whether or not the points in this point cloud are selectable.", display_, @@ -637,6 +643,26 @@ void PointCloudCommon::update(float /*wall_dt*/, float /*ros_dt*/) new_color_transformer_ = false; } + if (continuous_transform_property_->getBool()) + { + for (CloudInfoPtr& cloud_info : cloud_infos_) + { + if (!context_->getFrameManager()->getTransform(cloud_info->message_->header.frame_id, ros::Time(), + cloud_info->position_, cloud_info->orientation_)) + { + std::stringstream ss; + ss << "Failed to transform from frame [" << cloud_info->message_->header.frame_id + << "] to frame [" << context_->getFrameManager()->getFixedFrame() << "]"; + display_->setStatusStd(StatusProperty::Error, "Message", ss.str()); + } + else + { + cloud_info->scene_node_->setPosition(cloud_info->position_); + cloud_info->scene_node_->setOrientation(cloud_info->orientation_); + } + } + } + updateStatus(); } diff --git a/src/rviz/default_plugin/point_cloud_common.h b/src/rviz/default_plugin/point_cloud_common.h index 173a6b41f5..98eebdb125 100644 --- a/src/rviz/default_plugin/point_cloud_common.h +++ b/src/rviz/default_plugin/point_cloud_common.h @@ -134,6 +134,7 @@ class PointCloudCommon : public QObject bool auto_size_; + BoolProperty* continuous_transform_property_; BoolProperty* selectable_property_; FloatProperty* point_world_size_property_; FloatProperty* point_pixel_size_property_; diff --git a/src/rviz/default_plugin/robot_model_display.cpp b/src/rviz/default_plugin/robot_model_display.cpp index 267043660d..91b210de4d 100644 --- a/src/rviz/default_plugin/robot_model_display.cpp +++ b/src/rviz/default_plugin/robot_model_display.cpp @@ -227,8 +227,27 @@ void RobotModelDisplay::update(float wall_dt, float /*ros_dt*/) float rate = update_rate_property_->getFloat(); bool update = rate < 0.0001f || time_since_last_transform_ >= rate; - if (has_new_transforms_ || update) + if (robot_->getRootLink() && (has_new_transforms_ || update)) { + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (context_->getFrameManager()->getTransform(robot_->getRootLink()->getName(), ros::Time(), + position, orientation)) + { + robot_->setPosition(position); + robot_->setOrientation(orientation); + linkUpdaterStatusFunction(StatusProperty::Ok, robot_->getRootLink()->getName(), "Transform OK", + this); + } + else + { + std::stringstream ss; + ss << "No transform from [" << robot_->getRootLink()->getName() << "] to [" + << fixed_frame_.toStdString() << "]"; + linkUpdaterStatusFunction(StatusProperty::Error, robot_->getRootLink()->getName(), ss.str(), this); + } + + robot_->update(TFLinkUpdater(context_->getFrameManager(), boost::bind(linkUpdaterStatusFunction, _1, _2, _3, this), tf_prefix_property_->getStdString())); diff --git a/src/rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.cpp b/src/rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.cpp index 03898d89d3..75755b82b0 100644 --- a/src/rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.cpp +++ b/src/rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.cpp @@ -49,6 +49,7 @@ FixedOrientationOrthoViewController::FixedOrientationOrthoViewController() : dra { scale_property_ = new FloatProperty("Scale", 10, "How much to scale up the size of things in the scene.", this); + scale_property_->setMin(1e-14); angle_property_ = new FloatProperty("Angle", 0, "Angle around the Z axis to rotate.", this); x_property_ = new FloatProperty("X", 0, "X component of camera position.", this); y_property_ = new FloatProperty("Y", 0, "Y component of camera position.", this); diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index 9c3acd808f..145839a439 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -47,7 +47,7 @@ FrameManager::FrameManager(std::shared_ptr tf_buffer, tf_buffer ? std::move(tf_buffer) : std::make_shared(ros::Duration(10 * 60)); tf_listener_ = tf_listener ? std::move(tf_listener) : - std::make_shared(*tf_buffer_, ros::NodeHandle(), true); + std::make_shared(*tf_buffer_, ros::NodeHandle(), false); setSyncMode(SyncOff); setPause(false); diff --git a/src/rviz/robot/link_updater.h b/src/rviz/robot/link_updater.h index c34285e689..6df20c38f3 100644 --- a/src/rviz/robot/link_updater.h +++ b/src/rviz/robot/link_updater.h @@ -40,7 +40,8 @@ namespace rviz class LinkUpdater { public: - virtual bool getLinkTransforms(const std::string& link_name, + virtual bool getLinkTransforms(const std::string& parent_link_name, + const std::string& link_name, Ogre::Vector3& visual_position, Ogre::Quaternion& visual_orientation, Ogre::Vector3& collision_position, diff --git a/src/rviz/robot/robot.cpp b/src/rviz/robot/robot.cpp index 36f5f45353..1dc926f12b 100644 --- a/src/rviz/robot/robot.cpp +++ b/src/rviz/robot/robot.cpp @@ -66,6 +66,8 @@ Robot::Robot(Ogre::SceneNode* root_node, , robot_loaded_(false) , inChangedEnableAllLinks(false) , name_(name) + , root_link_(nullptr) + , alpha_(1.f) { root_visual_node_ = root_node->createChildSceneNode(); root_collision_node_ = root_node->createChildSceneNode(); @@ -218,11 +220,14 @@ void Robot::clear() RobotLink* Robot::LinkFactory::createLink(Robot* robot, const urdf::LinkConstSharedPtr& link, + Ogre::SceneNode* parent_visual_node, + Ogre::SceneNode* parent_collision_node, const std::string& parent_joint_name, bool visual, bool collision) { - return new RobotLink(robot, link, parent_joint_name, visual, collision); + return new RobotLink(robot, link, parent_visual_node, parent_collision_node, parent_joint_name, visual, + collision); } RobotJoint* Robot::LinkFactory::createJoint(Robot* robot, const urdf::JointConstSharedPtr& joint) @@ -244,12 +249,16 @@ void Robot::load(const urdf::ModelInterface& urdf, bool visual, bool collision) // Create properties for each link. // Properties are not added to display until changedLinkTreeStyle() is called (below). { - typedef std::map M_NameToUrdfLink; - M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin(); - M_NameToUrdfLink::const_iterator link_end = urdf.links_.end(); - for (; link_it != link_end; ++link_it) + // traverse URDF tree in depth first order and copy tree structure for links' scene nodes + std::vector> link_stack_; + link_stack_.emplace_back(urdf.getRoot(), root_visual_node_, root_collision_node_); + while (!link_stack_.empty()) { - const urdf::LinkConstSharedPtr& urdf_link = link_it->second; + urdf::LinkConstSharedPtr urdf_link = std::get<0>(link_stack_.back()); + Ogre::SceneNode* parent_visual_node = std::get<1>(link_stack_.back()); + Ogre::SceneNode* parent_collision_node = std::get<2>(link_stack_.back()); + link_stack_.pop_back(); + std::string parent_joint_name; if (urdf_link != urdf.getRoot() && urdf_link->parent_joint) @@ -257,7 +266,9 @@ void Robot::load(const urdf::ModelInterface& urdf, bool visual, bool collision) parent_joint_name = urdf_link->parent_joint->name; } - RobotLink* link = link_factory_->createLink(this, urdf_link, parent_joint_name, visual, collision); + RobotLink* link = + link_factory_->createLink(this, urdf_link, parent_visual_node, parent_collision_node, + parent_joint_name, visual, collision); if (urdf_link == urdf.getRoot()) { @@ -267,6 +278,9 @@ void Robot::load(const urdf::ModelInterface& urdf, bool visual, bool collision) links_[urdf_link->name] = link; link->setRobotAlpha(alpha_); + + for (const auto& c : urdf_link->child_links) + link_stack_.emplace_back(c, link->getVisualTreeNode(), link->getCollisionTreeNode()); } } @@ -709,8 +723,8 @@ void Robot::update(const LinkUpdater& updater) Ogre::Vector3 visual_position, collision_position; Ogre::Quaternion visual_orientation, collision_orientation; - if (updater.getLinkTransforms(link->getName(), visual_position, visual_orientation, - collision_position, collision_orientation)) + if (updater.getLinkTransforms(link->getParentLinkName(), link->getName(), visual_position, + visual_orientation, collision_position, collision_orientation)) { // Check if visual_orientation, visual_position, collision_orientation, and collision_position are // NaN. diff --git a/src/rviz/robot/robot.h b/src/rviz/robot/robot.h index aabab1ef34..c433586f8c 100644 --- a/src/rviz/robot/robot.h +++ b/src/rviz/robot/robot.h @@ -199,6 +199,8 @@ class Robot : public QObject virtual RobotLink* createLink(Robot* robot, const urdf::LinkConstSharedPtr& link, + Ogre::SceneNode* parent_visual_node, + Ogre::SceneNode* parent_collision_node, const std::string& parent_joint_name, bool visual, bool collision); diff --git a/src/rviz/robot/robot_link.cpp b/src/rviz/robot/robot_link.cpp index 9037db0279..af9e8a1f7d 100644 --- a/src/rviz/robot/robot_link.cpp +++ b/src/rviz/robot/robot_link.cpp @@ -151,6 +151,8 @@ static std::map errors; RobotLink::RobotLink(Robot* robot, const urdf::LinkConstSharedPtr& link, + Ogre::SceneNode* parent_visual_node, + Ogre::SceneNode* parent_collision_node, const std::string& parent_joint_name, bool visual, bool collision) @@ -158,12 +160,14 @@ RobotLink::RobotLink(Robot* robot, , scene_manager_(robot->getDisplayContext()->getSceneManager()) , context_(robot->getDisplayContext()) , name_(link->name) + , parent_link_name_(link->getParent() ? link->getParent()->name : "") , parent_joint_name_(parent_joint_name) , visual_node_(nullptr) , collision_node_(nullptr) , trail_(nullptr) , axes_(nullptr) , material_alpha_(1.0) + , use_original_material(true) , robot_alpha_(1.0) , only_render_depth_(false) , is_selectable_(true) @@ -198,8 +202,12 @@ RobotLink::RobotLink(Robot* robot, link_property_->collapse(); - visual_node_ = robot_->getVisualNode()->createChildSceneNode(); - collision_node_ = robot_->getCollisionNode()->createChildSceneNode(); + // create scene nodes in a tree structure analog to URDF tree + // we use visual_node_ and collision_node_ as leafs in order not to hide child links when set to invisible + visual_tree_node_ = parent_visual_node->createChildSceneNode(); + visual_node_ = visual_tree_node_->createChildSceneNode(); + collision_tree_node_ = parent_collision_node->createChildSceneNode(); + collision_node_ = collision_tree_node_->createChildSceneNode(); // create material for coloring links color_material_ = Ogre::MaterialPtr(new Ogre::Material( @@ -306,6 +314,8 @@ RobotLink::~RobotLink() scene_manager_->destroySceneNode(visual_node_); scene_manager_->destroySceneNode(collision_node_); + scene_manager_->destroySceneNode(visual_tree_node_); + scene_manager_->destroySceneNode(collision_tree_node_); if (trail_) { @@ -404,11 +414,13 @@ void RobotLink::updateAlpha() { material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); material->setDepthWriteEnabled(false); + use_original_material = false; } else { material->setSceneBlending(Ogre::SBT_REPLACE); material->setDepthWriteEnabled(true); + use_original_material = true; } } } @@ -668,6 +680,7 @@ void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& l sub->setMaterial(mat); } materials_[sub] = sub->getMaterial(); + original_materials_[sub] = sub->getMaterial()->clone(sub->getMaterial()->getName() + "_original"); } } } @@ -872,16 +885,16 @@ void RobotLink::setTransforms(const Ogre::Vector3& visual_position, const Ogre::Vector3& collision_position, const Ogre::Quaternion& collision_orientation) { - if (visual_node_) + if (visual_tree_node_) { - visual_node_->setPosition(visual_position); - visual_node_->setOrientation(visual_orientation); + visual_tree_node_->setPosition(visual_position); + visual_tree_node_->setOrientation(visual_orientation); } - if (collision_node_) + if (collision_tree_node_) { - collision_node_->setPosition(collision_position); - collision_node_->setOrientation(collision_orientation); + collision_tree_node_->setPosition(collision_position); + collision_tree_node_->setOrientation(collision_orientation); } position_property_->setVector(visual_position); @@ -925,9 +938,10 @@ void RobotLink::setToNormalMaterial() { M_SubEntityToMaterial::iterator it = materials_.begin(); M_SubEntityToMaterial::iterator end = materials_.end(); - for (; it != end; ++it) + M_SubEntityToMaterial::iterator it_original = original_materials_.begin(); + for (; it != end; ++it, ++it_original) { - it->first->setMaterial(it->second); + it->first->setMaterial(use_original_material ? it_original->second : it->second); } } } diff --git a/src/rviz/robot/robot_link.h b/src/rviz/robot/robot_link.h index 7c57491f1a..54a474e038 100644 --- a/src/rviz/robot/robot_link.h +++ b/src/rviz/robot/robot_link.h @@ -82,6 +82,8 @@ class RobotLink : public QObject public: RobotLink(Robot* robot, const urdf::LinkConstSharedPtr& link, + Ogre::SceneNode* parent_visual_node, + Ogre::SceneNode* parent_collision_node, const std::string& parent_joint_name, bool visual, bool collision); @@ -99,6 +101,10 @@ class RobotLink : public QObject { return name_; } + const std::string& getParentLinkName() const + { + return parent_link_name_; + } const std::string& getParentJointName() const { return parent_joint_name_; @@ -119,6 +125,14 @@ class RobotLink : public QObject { return collision_node_; } + Ogre::SceneNode* getVisualTreeNode() const + { + return visual_tree_node_; + } + Ogre::SceneNode* getCollisionTreeNode() const + { + return collision_tree_node_; + } Robot* getRobot() const { return robot_; @@ -200,6 +214,7 @@ private Q_SLOTS: DisplayContext* context_; std::string name_; ///< Name of this link + std::string parent_link_name_; std::string parent_joint_name_; std::vector child_joint_names_; @@ -216,8 +231,10 @@ private Q_SLOTS: private: typedef std::map M_SubEntityToMaterial; M_SubEntityToMaterial materials_; + M_SubEntityToMaterial original_materials_; Ogre::MaterialPtr default_material_; std::string default_material_name_; + bool use_original_material; std::vector visual_meshes_; ///< The entities representing the visual mesh of this link (if they exist) @@ -226,6 +243,8 @@ private Q_SLOTS: Ogre::SceneNode* visual_node_; ///< The scene node the visual meshes are attached to Ogre::SceneNode* collision_node_; ///< The scene node the collision meshes are attached to + Ogre::SceneNode* visual_tree_node_; ///< The scene node above visual_node is attached to + Ogre::SceneNode* collision_tree_node_; ///< The scene node above collision_node is attached to Ogre::RibbonTrail* trail_; diff --git a/src/rviz/robot/tf_link_updater.cpp b/src/rviz/robot/tf_link_updater.cpp index 7a378ebf71..8669100fcf 100644 --- a/src/rviz/robot/tf_link_updater.cpp +++ b/src/rviz/robot/tf_link_updater.cpp @@ -43,24 +43,47 @@ TFLinkUpdater::TFLinkUpdater(FrameManager* frame_manager, { } -bool TFLinkUpdater::getLinkTransforms(const std::string& _link_name, +bool TFLinkUpdater::getLinkTransforms(const std::string& _parent_link_name, + const std::string& _link_name, Ogre::Vector3& visual_position, Ogre::Quaternion& visual_orientation, Ogre::Vector3& collision_position, Ogre::Quaternion& collision_orientation) const { + if (_parent_link_name.empty()) + { + visual_position = Ogre::Vector3::ZERO; + visual_orientation = Ogre::Quaternion::IDENTITY; + collision_position = Ogre::Vector3::ZERO; + collision_orientation = Ogre::Quaternion::IDENTITY; + return true; + } + std::string parent_link_name = concat(tf_prefix_, _parent_link_name); std::string link_name = concat(tf_prefix_, _link_name); Ogre::Vector3 position; Ogre::Quaternion orientation; - if (!frame_manager_->getTransform(link_name, ros::Time(), position, orientation)) + + geometry_msgs::TransformStamped tf; + try + { + tf = frame_manager_->getTF2BufferPtr()->lookupTransform(parent_link_name, link_name, ros::Time()); + } + catch (std::runtime_error& e) { std::stringstream ss; ss << "No transform from [" << link_name << "] to [" << frame_manager_->getFixedFrame() << "]"; setLinkStatus(StatusProperty::Error, link_name, ss.str()); + ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", link_name.c_str(), + parent_link_name.c_str(), e.what()); return false; } + position = + Ogre::Vector3(tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z); + orientation = Ogre::Quaternion(tf.transform.rotation.w, tf.transform.rotation.x, + tf.transform.rotation.y, tf.transform.rotation.z); + setLinkStatus(StatusProperty::Ok, link_name, "Transform OK"); // Collision/visual transforms are the same in this case diff --git a/src/rviz/robot/tf_link_updater.h b/src/rviz/robot/tf_link_updater.h index 39726a4ffb..4c5aa4deb2 100644 --- a/src/rviz/robot/tf_link_updater.h +++ b/src/rviz/robot/tf_link_updater.h @@ -52,7 +52,8 @@ class TFLinkUpdater : public LinkUpdater TFLinkUpdater(FrameManager* frame_manager, const StatusCallback& status_cb = StatusCallback(), const std::string& tf_prefix = std::string()); - bool getLinkTransforms(const std::string& link_name, + bool getLinkTransforms(const std::string& parent_link_name, + const std::string& link_name, Ogre::Vector3& visual_position, Ogre::Quaternion& visual_orientation, Ogre::Vector3& collision_position, diff --git a/src/test/odometry.py b/src/test/odometry.py new file mode 100644 index 0000000000..04ec0d4758 --- /dev/null +++ b/src/test/odometry.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python + +import math +import time + +import rospy +from geometry_msgs.msg import TransformStamped +from visualization_msgs.msg import Marker +from nav_msgs.msg import Odometry +from tf.transformations import quaternion_from_euler +from tf2_ros.transform_broadcaster import TransformBroadcaster + +RATE = 30 + + +def odom_to_tf(odom_msg): + tf = TransformStamped() + tf.header = odom_msg.header + tf.child_frame_id = odom_msg.child_frame_id + tf.transform.translation = odom_msg.pose.pose.position + tf.transform.rotation = odom_msg.pose.pose.orientation + tf_broadcaster.sendTransform(tf) + + +def draw_eight(elapsed_time, full_cycle_duration=10, scale=10): + # lemniscate of Gerono + progress = (elapsed_time % full_cycle_duration) / full_cycle_duration + t = -0.5 * math.pi + progress * (2 * math.pi) + x = math.cos(t) * scale + y = math.sin(t) * math.cos(t) * scale + dx_dt = -math.sin(t) + dy_dt = math.cos(t) * math.cos(t) - math.sin(t) * math.sin(t) + yaw = math.atan2(dy_dt, dx_dt) + return x, y, yaw + + +def display_dummy_robot(stamp): + marker = Marker() + marker.header.frame_id = "base_link" + marker.header.stamp = stamp + marker.ns = "robot" + marker.id = 0 + marker.type = Marker.CUBE + marker.action = Marker.ADD + marker.pose.position.x = 0 + marker.pose.position.y = 0 + marker.pose.position.z = 0 + marker.pose.orientation.x = 0.0 + marker.pose.orientation.y = 0.0 + marker.pose.orientation.z = 0.0 + marker.pose.orientation.w = 1.0 + marker.scale.x = 1 + marker.scale.y = 1 + marker.scale.z = 1 + marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + vis_pub.publish(marker) + + +rospy.init_node("odometry_test", anonymous=True) + +tf_broadcaster = TransformBroadcaster() + +odom_pub = rospy.Publisher("robot/odom", Odometry, queue_size=5) +vis_pub = rospy.Publisher("robot/marker", Marker, queue_size=5) + +time.sleep(1) + +rate = rospy.Rate(RATE) +start = rospy.Time.now() +while not rospy.is_shutdown(): + now = rospy.Time.now() + + robot_x, robot_y, robot_yaw = draw_eight((now - start).to_sec()) + q = quaternion_from_euler(0, 0, robot_yaw) + + odom_robot = Odometry() + odom_robot.header.stamp = now + odom_robot.header.frame_id = "world" + odom_robot.child_frame_id = "base_link" + odom_robot.pose.pose.position.x = robot_x + odom_robot.pose.pose.position.y = robot_y + odom_robot.pose.pose.orientation.x = q[0] + odom_robot.pose.pose.orientation.y = q[1] + odom_robot.pose.pose.orientation.z = q[2] + odom_robot.pose.pose.orientation.w = q[3] + + odom_to_tf(odom_robot) + + odom_pub.publish(odom_robot) + + display_dummy_robot(now) + + rate.sleep() diff --git a/src/test/odometry.rviz b/src/test/odometry.rviz new file mode 100755 index 0000000000..9691a59af4 --- /dev/null +++ b/src/test/odometry.rviz @@ -0,0 +1,165 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Odometry1 + - /Odometry1/Shape1 + Splitter Ratio: 0.6264705657958984 + Tree Height: 818 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Angle Tolerance: 0.5 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 52 + Name: Odometry + Position Tolerance: 1 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /robot/odom + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /robot/marker + Name: Marker + Namespaces: + robot: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 20.20941925048828 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 1.479522943496704 + Y: 0.4954601228237152 + Z: -1.4441502094268799 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7553982138633728 + Target Frame: + Yaw: 0.8353981971740723 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1115 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000003bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003bd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003bd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007760000003efc0100000002fb0000000800540069006d00650100000000000007760000025600fffffffb0000000800540069006d0065010000000000000450000000000000000000000505000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1910 + X: 0 + Y: 25 diff --git a/src/test/point_cloud_test.py b/src/test/point_cloud_test.py new file mode 100644 index 0000000000..a410c9c33b --- /dev/null +++ b/src/test/point_cloud_test.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python + +# This program publishes a pointcloud2 to test rviz rendering + +from __future__ import print_function + +import rospy +import numpy as np +from sensor_msgs import point_cloud2 +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from std_msgs.msg import Header + +import math +import time +from geometry_msgs.msg import TransformStamped +from tf2_ros.transform_broadcaster import TransformBroadcaster +from tf.transformations import quaternion_from_euler +from visualization_msgs.msg import Marker + +RATE = 30 +width = 100 +height = 100 + + +def generate_point_cloud(): + fields = [ + PointField("x", 0, PointField.FLOAT32, 1), + PointField("y", 4, PointField.FLOAT32, 1), + PointField("z", 8, PointField.FLOAT32, 1), + PointField("intensity", 12, PointField.FLOAT32, 1), + ] + + header = Header() + header.frame_id = "map" + header.stamp = rospy.Time.now() + + x, y = np.meshgrid(np.linspace(-2, 2, width), np.linspace(-2, 2, height)) + z = 0.5 * np.sin(2 * x) * np.sin(3 * y) + points = np.array([x, y, z, z]).reshape(4, -1).T + + return point_cloud2.create_cloud(header, fields, points) + + +def draw_eight(elapsed_time, full_cycle_duration=10, scale=10): + # lemniscate of Gerono + progress = (elapsed_time % full_cycle_duration) / full_cycle_duration + t = -0.5 * math.pi + progress * (2 * math.pi) + x = math.cos(t) * scale + y = math.sin(t) * math.cos(t) * scale + dx_dt = -math.sin(t) + dy_dt = math.cos(t) * math.cos(t) - math.sin(t) * math.sin(t) + yaw = math.atan2(dy_dt, dx_dt) + return x, y, yaw + + +def display_dummy_robot(stamp): + marker = Marker() + marker.header.frame_id = "base_link" + marker.header.stamp = stamp + marker.ns = "robot" + marker.id = 0 + marker.type = Marker.CUBE + marker.action = Marker.ADD + marker.pose.position.x = 0 + marker.pose.position.y = 0 + marker.pose.position.z = 0 + marker.pose.orientation.x = 0.0 + marker.pose.orientation.y = 0.0 + marker.pose.orientation.z = 0.0 + marker.pose.orientation.w = 1.0 + marker.scale.x = 1 + marker.scale.y = 1 + marker.scale.z = 1 + marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + vis_pub.publish(marker) + + +if __name__ == "__main__": + rospy.init_node("point_cloud_test") + + tf_broadcaster = TransformBroadcaster() + + vis_pub = rospy.Publisher("robot/marker", Marker, queue_size=5) + pc_pub = rospy.Publisher("points2", PointCloud2, queue_size=5) + pc = generate_point_cloud() + + i = 0 + rate = rospy.Rate(RATE) + start = rospy.Time.now() + while not rospy.is_shutdown(): + now = rospy.Time.now() + + if i % RATE == 0: + # publish just once per second + pc.header.stamp = now + pc_pub.publish(pc) + i += 1 + + robot_x, robot_y, robot_yaw = draw_eight((now - start).to_sec()) + q = quaternion_from_euler(0, 0, robot_yaw) + + tf = TransformStamped() + tf.header.frame_id = "map" + tf.header.stamp = now + tf.child_frame_id = "base_link" + tf.transform.translation.x = robot_x + tf.transform.translation.y = robot_y + tf.transform.rotation.x = q[0] + tf.transform.rotation.y = q[1] + tf.transform.rotation.z = q[2] + tf.transform.rotation.w = q[3] + tf_broadcaster.sendTransform(tf) + + display_dummy_robot(now) + + rate.sleep() diff --git a/src/test/point_cloud_test.rviz b/src/test/point_cloud_test.rviz new file mode 100644 index 0000000000..f6d68c3465 --- /dev/null +++ b/src/test/point_cloud_test.rviz @@ -0,0 +1,155 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 818 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: map + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /points2 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /robot/marker + Name: Marker + Namespaces: + robot: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 30.52842903137207 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6153981685638428 + Target Frame: + Yaw: 1.025397777557373 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1115 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000003bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003bd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003bd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0