From ea17ce3b8c160d44a04c281f0243a6457e62df06 Mon Sep 17 00:00:00 2001 From: Andreas Reich Date: Wed, 15 Dec 2021 12:48:41 +0100 Subject: [PATCH 01/10] odometry_display: add option to continuously transform the visuals w.r.t. fixed frame This option is particularly useful for messages, whose frame is moving w.r.t. fixed frame. --- src/rviz/default_plugin/odometry_display.cpp | 46 +++++++++++++++++--- src/rviz/default_plugin/odometry_display.h | 3 ++ 2 files changed, 43 insertions(+), 6 deletions(-) diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp index 6fb6fac3e9..a04f4941f7 100644 --- a/src/rviz/default_plugin/odometry_display.cpp +++ b/src/rviz/default_plugin/odometry_display.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -47,6 +48,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.", @@ -301,14 +308,25 @@ 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... + // 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 + const geometry_msgs::Pose& p = message->pose.pose; + position = Ogre::Vector3(p.position.x, p.position.y, p.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 +390,22 @@ void OdometryDisplay::update(float /*wall_dt*/, float /*ros_dt*/) assert(arrows_.size() == axes_.size()); assert(axes_.size() == covariance_property_->sizeVisual()); + + // applyTransforms + if (!continuous_transform_property_->getBool() || !last_used_message_) + return; + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(last_used_message_->header.frame_id, ros::Time(), + 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..3b4c46776a 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; @@ -103,6 +104,8 @@ private Q_SLOTS: nav_msgs::Odometry::ConstPtr last_used_message_; + rviz::BoolProperty* continuous_transform_property_; + rviz::EnumProperty* shape_property_; rviz::ColorProperty* color_property_; From 63b838765da4a10ccf3384d928c663abd96d9dd8 Mon Sep 17 00:00:00 2001 From: Andreas Reich Date: Wed, 15 Dec 2021 12:50:47 +0100 Subject: [PATCH 02/10] odometry_display: add test script and rviz config to verify correct behavior of "continuous transform" option --- src/test/odometry.py | 96 ++++++++++++++++++++++++ src/test/odometry.rviz | 165 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 261 insertions(+) create mode 100644 src/test/odometry.py create mode 100755 src/test/odometry.rviz 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 From 9ccaddc6da28bfc5fff4e4905ab3df5ca4105ad2 Mon Sep 17 00:00:00 2001 From: Andreas Reich Date: Wed, 15 Dec 2021 14:19:15 +0100 Subject: [PATCH 03/10] odometry_display: introduce ref pose to handle very large positional values in odometry message without getting numerical issues These large positional values can occur when the odometry message comes from a GNSS receiver where GPS positions are converted into UTM coordinates. --- CMakeLists.txt | 1 + package.xml | 1 + src/rviz/default_plugin/odometry_display.cpp | 53 ++++++++++++-------- src/rviz/default_plugin/odometry_display.h | 1 + 4 files changed, 36 insertions(+), 20 deletions(-) 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 a04f4941f7..f339326b2f 100644 --- a/src/rviz/default_plugin/odometry_display.cpp +++ b/src/rviz/default_plugin/odometry_display.cpp @@ -27,6 +27,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include + #include #include #include @@ -160,6 +162,10 @@ void OdometryDisplay::clear() { last_used_message_.reset(); } + if (ref_pose_) + { + ref_pose_.reset(); + } } void OdometryDisplay::updateColorAndAlpha() @@ -285,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; @@ -323,8 +324,20 @@ void OdometryDisplay::processMessage(const nav_msgs::Odometry::ConstPtr& message else { // put the visuals at the pose specified by the message and transform their scene node continuously + + // 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, p.position.y, p.position.z); + 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); } @@ -391,14 +404,14 @@ void OdometryDisplay::update(float /*wall_dt*/, float /*ros_dt*/) assert(arrows_.size() == axes_.size()); assert(axes_.size() == covariance_property_->sizeVisual()); - // applyTransforms - if (!continuous_transform_property_->getBool() || !last_used_message_) + // 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()->getTransform(last_used_message_->header.frame_id, ros::Time(), - position, 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; diff --git a/src/rviz/default_plugin/odometry_display.h b/src/rviz/default_plugin/odometry_display.h index 3b4c46776a..731b2130af 100644 --- a/src/rviz/default_plugin/odometry_display.h +++ b/src/rviz/default_plugin/odometry_display.h @@ -103,6 +103,7 @@ private Q_SLOTS: D_Axes axes_; nav_msgs::Odometry::ConstPtr last_used_message_; + geometry_msgs::Pose::Ptr ref_pose_; rviz::BoolProperty* continuous_transform_property_; From 73e977ede9f90d8923d4659b5ff8bb162a6ad861 Mon Sep 17 00:00:00 2001 From: Andreas Reich Date: Wed, 15 Dec 2021 14:49:28 +0100 Subject: [PATCH 04/10] point_cloud_common: add option to continuously transform the visuals w.r.t. fixed frame This option is particularly useful for messages, whose frame is moving w.r.t. fixed frame. --- .../default_plugin/point_cloud_common.cpp | 26 +++++++++++++++++++ src/rviz/default_plugin/point_cloud_common.h | 1 + 2 files changed, 27 insertions(+) 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_; From ac6b080e480d157c16d07aecf40e471d4b242cc8 Mon Sep 17 00:00:00 2001 From: Andreas Reich Date: Wed, 15 Dec 2021 14:50:43 +0100 Subject: [PATCH 05/10] point_cloud_common: add test script and rviz config to verify correct behavior of "continuous transform" option --- src/test/point_cloud_test.py | 120 +++++++++++++++++++++++++ src/test/point_cloud_test.rviz | 155 +++++++++++++++++++++++++++++++++ 2 files changed, 275 insertions(+) create mode 100644 src/test/point_cloud_test.py create mode 100644 src/test/point_cloud_test.rviz 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 From e22bf62dac35ff7e09d27a4f91f5c9c68206ad5e Mon Sep 17 00:00:00 2001 From: Andreas Reich Date: Tue, 28 Dec 2021 15:56:06 +0100 Subject: [PATCH 06/10] frame_manager: disable separate thread for tf listener to avoid tf race conditions during update cycle of all displays and cameras During update cycle of displays and views (cameras) the tf buffer should not be updated. Otherwise it is possible that displays or cameras, which are updated at a later point in time within the cycle, use a more recent transform. This leads to undesired jittering of visuals. This affects displays, which use the latest available transform with ros::Time(). And all views/cameras as they always use the latest available transform. --- src/rviz/frame_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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); From c7229506e8580cd54b2100388d3c1b9e094b3b8c Mon Sep 17 00:00:00 2001 From: Andreas Reich Date: Tue, 28 Dec 2021 15:40:20 +0100 Subject: [PATCH 07/10] robot_model_display: arrange scene nodes of links in URDF's tree structure in order to avoid jittering of movable links due to tf latency. --- .../default_plugin/robot_model_display.cpp | 19 ++++++++++++ src/rviz/robot/link_updater.h | 3 +- src/rviz/robot/robot.cpp | 30 +++++++++++++------ src/rviz/robot/robot.h | 2 ++ src/rviz/robot/robot_link.cpp | 25 +++++++++++----- src/rviz/robot/robot_link.h | 17 +++++++++++ src/rviz/robot/tf_link_updater.cpp | 27 +++++++++++++++-- src/rviz/robot/tf_link_updater.h | 3 +- 8 files changed, 105 insertions(+), 21 deletions(-) diff --git a/src/rviz/default_plugin/robot_model_display.cpp b/src/rviz/default_plugin/robot_model_display.cpp index 267043660d..c35181669c 100644 --- a/src/rviz/default_plugin/robot_model_display.cpp +++ b/src/rviz/default_plugin/robot_model_display.cpp @@ -229,6 +229,25 @@ void RobotModelDisplay::update(float wall_dt, float /*ros_dt*/) if (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/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..f60d49830a 100644 --- a/src/rviz/robot/robot.cpp +++ b/src/rviz/robot/robot.cpp @@ -218,11 +218,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 +247,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 +264,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 +276,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 +721,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..7de9fb8376 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,6 +160,7 @@ 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) @@ -198,8 +201,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 +313,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_) { @@ -872,16 +881,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); diff --git a/src/rviz/robot/robot_link.h b/src/rviz/robot/robot_link.h index 7c57491f1a..070eeaeef2 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_; @@ -226,6 +241,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, From 97666ef89008e2770eb37daa880c8c90a888c69d Mon Sep 17 00:00:00 2001 From: Andreas Reich Date: Sat, 29 Jan 2022 19:14:05 +0100 Subject: [PATCH 08/10] Add support for transparent materials Previously all materials are set opaque when alpha option is set to 1. With this commit the materials are set to their original state when alpha is set to 1. --- src/rviz/robot/robot_link.cpp | 9 +++++++-- src/rviz/robot/robot_link.h | 2 ++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/rviz/robot/robot_link.cpp b/src/rviz/robot/robot_link.cpp index 7de9fb8376..af9e8a1f7d 100644 --- a/src/rviz/robot/robot_link.cpp +++ b/src/rviz/robot/robot_link.cpp @@ -167,6 +167,7 @@ RobotLink::RobotLink(Robot* robot, , trail_(nullptr) , axes_(nullptr) , material_alpha_(1.0) + , use_original_material(true) , robot_alpha_(1.0) , only_render_depth_(false) , is_selectable_(true) @@ -413,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; } } } @@ -677,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"); } } } @@ -934,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 070eeaeef2..54a474e038 100644 --- a/src/rviz/robot/robot_link.h +++ b/src/rviz/robot/robot_link.h @@ -231,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) From 1097c55a83f9052bdf2ffd86f730239dfda98538 Mon Sep 17 00:00:00 2001 From: anre Date: Tue, 22 Mar 2022 11:23:17 +0100 Subject: [PATCH 09/10] Fix crash because of uninizialized root_link_ in robot_model_display --- src/rviz/default_plugin/robot_model_display.cpp | 2 +- src/rviz/robot/robot.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/rviz/default_plugin/robot_model_display.cpp b/src/rviz/default_plugin/robot_model_display.cpp index c35181669c..91b210de4d 100644 --- a/src/rviz/default_plugin/robot_model_display.cpp +++ b/src/rviz/default_plugin/robot_model_display.cpp @@ -227,7 +227,7 @@ 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; diff --git a/src/rviz/robot/robot.cpp b/src/rviz/robot/robot.cpp index f60d49830a..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(); From 54e66489312532e3c188bcda989c8cc12c08133c Mon Sep 17 00:00:00 2001 From: anre Date: Sun, 29 May 2022 13:05:45 +0200 Subject: [PATCH 10/10] Set lower limit for scale in FixedOrientationOrthoViewController --- .../view_controllers/fixed_orientation_ortho_view_controller.cpp | 1 + 1 file changed, 1 insertion(+) 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);