Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

MoveIt! tf2 migration #830

Merged
merged 6 commits into from May 18, 2018
Merged

MoveIt! tf2 migration #830

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
f4ebdc6
tf2 migration, resolves issue #745
IanTheEngineer May 17, 2018
363918e
use new tf2 API in rviz to avoid deprecation warning
wjwwood May 10, 2018
c09d6bb
Replaced deprecated tf2 _getLatestCommonTime
IanTheEngineer May 17, 2018
3919c5d
Updates tf2 pointer checks based on review
IanTheEngineer May 17, 2018
8c3f2d7
tf2 migration clang updates
IanTheEngineer May 17, 2018
1cdd09d
fix class_loader.hpp
rhaschke May 17, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
5 changes: 3 additions & 2 deletions moveit_core/CMakeLists.txt
View file
Open in desktop
Expand Up @@ -32,7 +32,8 @@ find_package(urdfdom_headers REQUIRED)

find_package(catkin REQUIRED
COMPONENTS
eigen_conversions
tf2_eigen
tf2_kdl
eigen_stl_containers
geometric_shapes
geometry_msgs
Expand Down Expand Up @@ -111,7 +112,7 @@ catkin_package(
moveit_dynamics_solver
${OCTOMAP_LIBRARIES}
CATKIN_DEPENDS
eigen_conversions
tf2_eigen
eigen_stl_containers
geometric_shapes
geometry_msgs
Expand Down
8 changes: 4 additions & 4 deletions moveit_core/collision_detection/src/collision_tools.cpp
View file
Open in desktop
Expand Up @@ -35,7 +35,7 @@
/* Author: Ioan Sucan */

#include <moveit/collision_detection/collision_tools.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>

namespace collision_detection
{
Expand Down Expand Up @@ -272,8 +272,8 @@ void costSourceToMsg(const CostSource& cost_source, moveit_msgs::CostSource& msg

void contactToMsg(const Contact& contact, moveit_msgs::ContactInformation& msg)
{
tf::pointEigenToMsg(contact.pos, msg.position);
tf::vectorEigenToMsg(contact.normal, msg.normal);
msg.position = tf2::toMsg(contact.pos);
tf2::toMsg(contact.normal, msg.normal);
msg.depth = contact.depth;
msg.contact_body_1 = contact.body_name_1;
msg.contact_body_2 = contact.body_name_2;
Expand All @@ -291,4 +291,4 @@ void contactToMsg(const Contact& contact, moveit_msgs::ContactInformation& msg)
msg.body_type_2 = moveit_msgs::ContactInformation::WORLD_OBJECT;
}

} // end of namespace collision_detection
} // end of namespace collision_detection
5 changes: 2 additions & 3 deletions moveit_core/constraint_samplers/CMakeLists.txt
View file
Open in desktop
Expand Up @@ -26,10 +26,10 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
if(CATKIN_ENABLE_TESTING)
find_package(orocos_kdl REQUIRED)
find_package(angles REQUIRED)
find_package(tf_conversions REQUIRED)
find_package(tf2_kdl REQUIRED)
find_package(moveit_resources REQUIRED)

include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf_conversions_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS})
include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS})

catkin_add_gtest(test_constraint_samplers
test/test_constraint_samplers.cpp
Expand All @@ -41,7 +41,6 @@ if(CATKIN_ENABLE_TESTING)
${catkin_LIBRARIES}
${angles_LIBRARIES}
${orocos_kdl_LIBRARIES}
${tf_conversions_LIBRARIES}
${urdfdom_LIBRARIES}
${urdfdom_headers_LIBRARIES}
)
Expand Down
1 change: 0 additions & 1 deletion moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
View file
Open in desktop
Expand Up @@ -37,7 +37,6 @@
#include <moveit/constraint_samplers/default_constraint_samplers.h>
#include <set>
#include <cassert>
#include <eigen_conversions/eigen_msg.h>
#include <boost/bind.hpp>

namespace constraint_samplers
Expand Down
7 changes: 2 additions & 5 deletions moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp
View file
Open in desktop
Expand Up @@ -36,8 +36,7 @@

#include <geometry_msgs/PoseStamped.h>
#include <kdl_parser/kdl_parser.hpp>
#include <eigen_conversions/eigen_msg.h>
#include <eigen_conversions/eigen_kdl.h>
#include <tf2_kdl/tf2_kdl.h>
#include <algorithm>
#include <numeric>

Expand Down Expand Up @@ -340,9 +339,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose
return false;
}
KDL::Frame pose_desired;
Eigen::Affine3d tp;
tf::poseMsgToEigen(ik_pose, tp);
tf::transformEigenToKDL(tp, pose_desired);
tf2::fromMsg(ik_pose, pose_desired);

// Do the IK
KDL::JntArray jnt_pos_in;
Expand Down
1 change: 0 additions & 1 deletion moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h
View file
Open in desktop
Expand Up @@ -43,7 +43,6 @@
#include <kdl_parser/kdl_parser.hpp>

#include <angles/angles.h>
#include <tf_conversions/tf_kdl.h>

#include <moveit/macros/class_forward.h>
#include <moveit_msgs/GetPositionFK.h>
Expand Down
10 changes: 5 additions & 5 deletions moveit_core/distance_field/src/distance_field.cpp
View file
Open in desktop
Expand Up @@ -37,7 +37,7 @@
#include <moveit/distance_field/distance_field.h>
#include <moveit/distance_field/find_internal_points.h>
#include <geometric_shapes/body_operations.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <ros/console.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
Expand Down Expand Up @@ -229,7 +229,7 @@ void DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Aff
void DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
{
Eigen::Affine3d pose_e;
tf::poseMsgToEigen(pose, pose_e);
tf2::fromMsg(pose, pose_e);
addShapeToField(shape, pose_e);
}

Expand Down Expand Up @@ -310,8 +310,8 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_
const geometry_msgs::Pose& new_pose)
{
Eigen::Affine3d old_pose_e, new_pose_e;
tf::poseMsgToEigen(old_pose, old_pose_e);
tf::poseMsgToEigen(new_pose, new_pose_e);
tf2::fromMsg(old_pose, old_pose_e);
tf2::fromMsg(new_pose, new_pose_e);
moveShapeInField(shape, old_pose_e, new_pose_e);
}

Expand All @@ -329,7 +329,7 @@ void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen
void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
{
Eigen::Affine3d pose_e;
tf::poseMsgToEigen(pose, pose_e);
tf2::fromMsg(pose, pose_e);
removeShapeFromField(shape, pose_e);
}

Expand Down
4 changes: 2 additions & 2 deletions moveit_core/distance_field/test/test_distance_field.cpp
View file
Open in desktop
Expand Up @@ -40,7 +40,7 @@
#include <moveit/distance_field/propagation_distance_field.h>
#include <moveit/distance_field/find_internal_points.h>
#include <geometric_shapes/body_operations.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <octomap/octomap.h>
#include <ros/console.h>

Expand Down Expand Up @@ -511,7 +511,7 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints)
p.position.z = .5;

Eigen::Affine3d p_eigen;
tf::poseMsgToEigen(p, p_eigen);
tf2::fromMsg(p, p_eigen);

gradient_df.addShapeToField(&sphere, p_eigen);
// printBoth(gradient_df, numX, numY, numZ);
Expand Down
12 changes: 6 additions & 6 deletions moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
View file
Open in desktop
Expand Up @@ -41,7 +41,7 @@
#include <moveit/collision_detection_fcl/collision_robot_fcl.h>
#include <moveit/collision_detection_fcl/collision_world_fcl.h>
#include <boost/math/constants/constants.hpp>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <boost/bind.hpp>
#include <limits>
#include <memory>
Expand Down Expand Up @@ -319,7 +319,7 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co
}
constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
Eigen::Affine3d t;
tf::poseMsgToEigen(pc.constraint_region.primitive_poses[i], t);
tf2::fromMsg(pc.constraint_region.primitive_poses[i], t);
constraint_region_pose_.push_back(t);
if (mobile_frame_)
constraint_region_.back()->setPose(constraint_region_pose_.back());
Expand All @@ -346,7 +346,7 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co
}
constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
Eigen::Affine3d t;
tf::poseMsgToEigen(pc.constraint_region.mesh_poses[i], t);
tf2::fromMsg(pc.constraint_region.mesh_poses[i], t);
constraint_region_pose_.push_back(t);
if (mobile_frame_)
constraint_region_.back()->setPose(constraint_region_pose_.back());
Expand Down Expand Up @@ -504,7 +504,7 @@ bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint&
return false;
}
Eigen::Quaterniond q;
tf::quaternionMsgToEigen(oc.orientation, q);
tf2::fromMsg(oc.orientation, q);
if (fabs(q.norm() - 1.0) > 1e-3)
{
ROS_WARN_NAMED("kinematic_constraints", "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, "
Expand Down Expand Up @@ -698,7 +698,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc
points_.push_back(Eigen::Vector3d(x, y, 0.0));
}

tf::poseMsgToEigen(vc.target_pose.pose, target_pose_);
tf2::fromMsg(vc.target_pose.pose, target_pose_);

if (tf.isFixedFrame(vc.target_pose.header.frame_id))
{
Expand All @@ -715,7 +715,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc
mobile_target_frame_ = true;
}

tf::poseMsgToEigen(vc.sensor_pose.pose, sensor_pose_);
tf2::fromMsg(vc.sensor_pose.pose, sensor_pose_);

if (tf.isFixedFrame(vc.sensor_pose.header.frame_id))
{
Expand Down
1 change: 0 additions & 1 deletion moveit_core/kinematic_constraints/src/utils.cpp
View file
Open in desktop
Expand Up @@ -36,7 +36,6 @@

#include <moveit/kinematic_constraints/utils.h>
#include <geometric_shapes/solid_primitive_dims.h>
#include <eigen_conversions/eigen_msg.h>

moveit_msgs::Constraints kinematic_constraints::mergeConstraints(const moveit_msgs::Constraints& first,
const moveit_msgs::Constraints& second)
Expand Down
6 changes: 2 additions & 4 deletions moveit_core/kinematic_constraints/test/test_constraints.cpp
View file
Open in desktop
Expand Up @@ -38,7 +38,7 @@
#include <gtest/gtest.h>
#include <urdf_parser/urdf_parser.h>
#include <fstream>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <boost/filesystem/path.hpp>
#include <moveit_resources/config.h>

Expand Down Expand Up @@ -659,9 +659,7 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple)

ASSERT_TRUE(oc.getLinkModel());

geometry_msgs::Pose p;

tf::poseEigenToMsg(ks.getGlobalLinkTransform(oc.getLinkModel()->getName()), p);
geometry_msgs::Pose p = tf2::toMsg(ks.getGlobalLinkTransform(oc.getLinkModel()->getName()));

ocm.orientation = p.orientation;
ocm.header.frame_id = kmodel->getModelFrame();
Expand Down
7 changes: 4 additions & 3 deletions moveit_core/package.xml
View file
Open in desktop
Expand Up @@ -22,7 +22,6 @@
<build_depend>assimp</build_depend>
<build_depend>boost</build_depend>
<build_depend>eigen</build_depend>
<build_depend>eigen_conversions</build_depend>
<build_depend>eigen_stl_containers</build_depend>
<build_depend>libfcl-dev</build_depend>
<build_depend version_gte="0.5.2">geometric_shapes</build_depend>
Expand All @@ -43,13 +42,14 @@
<build_depend>shape_msgs</build_depend>
<build_depend>srdfdom</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf2_eigen</build_depend>
<build_depend>tf2_kdl</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>

<run_depend>assimp</run_depend>
<run_depend>boost</run_depend>
<run_depend>eigen</run_depend>
<run_depend>eigen_conversions</run_depend>
<run_depend>eigen_stl_containers</run_depend>
<run_depend>libfcl-dev</run_depend>
<run_depend version_gte="0.5.2">geometric_shapes</run_depend>
Expand All @@ -68,12 +68,13 @@
<run_depend>sensor_msgs</run_depend>
<run_depend>srdfdom</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf2_eigen</run_depend>
<run_depend>tf2_kdl</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>

<test_depend>angles</test_depend>
<test_depend>moveit_resources</test_depend>
<test_depend>orocos_kdl</test_depend>
<test_depend>rosunit</test_depend>
<test_depend>tf_conversions</test_depend>
</package>
28 changes: 13 additions & 15 deletions moveit_core/planning_scene/src/planning_scene.cpp
View file
Open in desktop
Expand Up @@ -44,7 +44,7 @@
#include <moveit/exceptions/exceptions.h>
#include <moveit/robot_state/attached_body.h>
#include <octomap_msgs/conversions.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <memory>
#include <set>

Expand Down Expand Up @@ -821,9 +821,7 @@ bool PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collisio
shapes::ShapeMsg sm;
if (constructMsgFromShape(obj->shapes_[j].get(), sm))
{
geometry_msgs::Pose p;
tf::poseEigenToMsg(obj->shape_poses_[j], p);

geometry_msgs::Pose p = tf2::toMsg(obj->shape_poses_[j]);
sv.setPoseMessage(&p);
boost::apply_visitor(sv, sm);
}
Expand Down Expand Up @@ -885,7 +883,7 @@ bool PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const
{
const shapes::OcTree* o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
octomap_msgs::fullMapToMsg(*o->octree, octomap.octomap);
tf::poseEigenToMsg(map->shape_poses_[0], octomap.origin);
octomap.origin = tf2::toMsg(map->shape_poses_[0]);
return true;
}
ROS_ERROR_NAMED("planning_scene",
Expand Down Expand Up @@ -1359,7 +1357,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map)
std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map.octomap)));
const Eigen::Affine3d& t = getTransforms().getTransform(map.header.frame_id);
Eigen::Affine3d p;
tf::poseMsgToEigen(map.origin, p);
tf2::fromMsg(map.origin, p);
p = t * p;
world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), p);
}
Expand Down Expand Up @@ -1500,7 +1498,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.object.primitive_poses[i], p);
tf2::fromMsg(object.object.primitive_poses[i], p);
shapes.push_back(shapes::ShapeConstPtr(s));
poses.push_back(p);
}
Expand All @@ -1511,7 +1509,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.object.mesh_poses[i], p);
tf2::fromMsg(object.object.mesh_poses[i], p);
shapes.push_back(shapes::ShapeConstPtr(s));
poses.push_back(p);
}
Expand All @@ -1522,7 +1520,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.object.plane_poses[i], p);
tf2::fromMsg(object.object.plane_poses[i], p);
shapes.push_back(shapes::ShapeConstPtr(s));
poses.push_back(p);
}
Expand Down Expand Up @@ -1698,7 +1696,7 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.primitive_poses[i], p);
tf2::fromMsg(object.primitive_poses[i], p);
world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
}
}
Expand All @@ -1708,7 +1706,7 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.mesh_poses[i], p);
tf2::fromMsg(object.mesh_poses[i], p);
world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
}
}
Expand All @@ -1718,7 +1716,7 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.plane_poses[i], p);
tf2::fromMsg(object.plane_poses[i], p);
world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
}
}
Expand Down Expand Up @@ -1754,19 +1752,19 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject
for (std::size_t i = 0; i < object.primitive_poses.size(); ++i)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.primitive_poses[i], p);
tf2::fromMsg(object.primitive_poses[i], p);
new_poses.push_back(t * p);
}
for (std::size_t i = 0; i < object.mesh_poses.size(); ++i)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.mesh_poses[i], p);
tf2::fromMsg(object.mesh_poses[i], p);
new_poses.push_back(t * p);
}
for (std::size_t i = 0; i < object.plane_poses.size(); ++i)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.plane_poses[i], p);
tf2::fromMsg(object.plane_poses[i], p);
new_poses.push_back(t * p);
}

Expand Down