tf2 compatibility for Noetic (#206)

This commit is contained in:
Robert Haschke 2020-09-21 21:43:34 +02:00 committed by GitHub
parent 1a2eafa256
commit dd9ce974ce
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 18 additions and 5 deletions

View File

@ -2,6 +2,11 @@ set(MOVEIT_LIB_NAME moveit_task_visualization_tools)
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_tools)
# TODO: Remove when Kinetic support is dropped
if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz supports TF2?
add_definitions(-DRVIZ_TF1)
endif()
set(HEADERS
${PROJECT_INCLUDE}/display_solution.h
${PROJECT_INCLUDE}/marker_visualization.h

View File

@ -7,6 +7,9 @@
#include <rviz/frame_manager.h>
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#ifndef RVIZ_TF1
#include <tf/tf.h>
#endif
#include <tf2_msgs/TF2Error.h>
#include <ros/console.h>
#include <eigen_conversions/eigen_msg.h>
@ -69,18 +72,23 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce
Ogre::Vector3 pos;
try {
#ifdef RVIZ_TF1
tf::TransformListener* tf = context->getFrameManager()->getTFClient();
std::string error_msg;
if (!tf->canTransform(planning_frame_, fixed_frame, ros::Time(), &error_msg)) {
ROS_WARN_STREAM_NAMED("MarkerVisualization", error_msg);
return false; // frame transform not (yet) available
}
tf::StampedTransform tm;
tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm);
auto q = tm.getRotation();
auto p = tm.getOrigin();
quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z());
pos = Ogre::Vector3(p.x(), p.y(), p.z());
#else
std::shared_ptr<tf2_ros::Buffer> tf = context->getFrameManager()->getTF2BufferPtr();
geometry_msgs::TransformStamped tm;
tm = tf->lookupTransform(planning_frame_, fixed_frame, ros::Time());
auto q = tm.transform.rotation;
auto p = tm.transform.translation;
quat = Ogre::Quaternion(q.w, -q.x, -q.y, -q.z);
pos = Ogre::Vector3(p.x, p.y, p.z);
#endif
} catch (const tf2::TransformException& e) {
ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what());
return false;