mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
tf2 compatibility for Noetic (#206)
This commit is contained in:
parent
1a2eafa256
commit
dd9ce974ce
@ -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
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user