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