move to tf2_eigen everywhere (#301)

v4hn · v4hn · commit 7ec874572c3b · 2021-10-18T23:34:29.000+02:00
diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt
@@ -3,7 +3,7 @@ project(moveit_task_constructor_core)
 
 find_package(Boost REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
-	eigen_conversions
+	tf2_eigen
 	geometry_msgs
 	moveit_core
 	moveit_ros_planning
diff --git a/core/package.xml b/core/package.xml
@@ -12,7 +12,7 @@
 	<build_depend>roscpp</build_depend>
 	<exec_depend>roscpp</exec_depend>
 
-	<depend>eigen_conversions</depend>
+	<depend>tf2_eigen</depend>
 	<depend>geometry_msgs</depend>
 	<depend>moveit_core</depend>
 	<depend>moveit_ros_planning</depend>
diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp
@@ -42,7 +42,7 @@
 #include <moveit/planning_pipeline/planning_pipeline.h>
 #include <moveit_msgs/MotionPlanRequest.h>
 #include <moveit/kinematic_constraints/utils.h>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -181,7 +181,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
 
 	geometry_msgs::PoseStamped target;
 	target.header.frame_id = from->getPlanningFrame();
-	tf::poseEigenToMsg(target_eigen, target.pose);
+	target.pose = tf2::toMsg(target_eigen);
 
 	req.goal_constraints.resize(1);
 	req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp
@@ -44,7 +44,7 @@
 #include <moveit/robot_state/robot_state.h>
 
 #include <Eigen/Geometry>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 #include <chrono>
 #include <functional>
 #include <iterator>
@@ -72,14 +72,14 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB
 void ComputeIK::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
 	geometry_msgs::PoseStamped pose_msg;
 	pose_msg.header.frame_id = link;
-	tf::poseEigenToMsg(pose, pose_msg.pose);
+	pose_msg.pose = tf2::toMsg(pose);
 	setIKFrame(pose_msg);
 }
 
 void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame) {
 	geometry_msgs::PoseStamped pose_msg;
 	pose_msg.header.frame_id = frame;
-	tf::poseEigenToMsg(pose, pose_msg.pose);
+	pose_msg.pose = tf2::toMsg(pose);
 	setTargetPose(pose_msg);
 }
 
@@ -259,7 +259,7 @@ void ComputeIK::compute() {
 		target_pose_msg.header.frame_id = scene->getPlanningFrame();
 
 	Eigen::Isometry3d target_pose;
-	tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
+	tf2::fromMsg(target_pose_msg.pose, target_pose);
 	if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) {
 		if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
 			ROS_WARN_STREAM_NAMED("ComputeIK",
@@ -286,7 +286,7 @@ void ComputeIK::compute() {
 	} else {
 		ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
 		Eigen::Isometry3d ik_pose;
-		tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
+		tf2::fromMsg(ik_pose_msg.pose, ik_pose);
 		if (robot_model->hasLinkModel(ik_pose_msg.header.frame_id)) {
 			link = robot_model->getLinkModel(ik_pose_msg.header.frame_id);
 		} else {
diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp
@@ -45,7 +45,7 @@
 
 #include <rviz_marker_tools/marker_creation.h>
 #include <Eigen/Geometry>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 #include <ros/console.h>
 
 namespace vm = visualization_msgs;
@@ -138,17 +138,16 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene&
 			// marker indicating correction
 			const cd::Contact& c = info.second.front();
 			rviz_marker_tools::setColor(m.color, failure ? rviz_marker_tools::RED : rviz_marker_tools::GREEN);
-			tf::poseEigenToMsg(Eigen::Translation3d(c.pos) *
-			                       Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), correction),
-			                   m.pose);
+			m.pose = tf2::toMsg(Eigen::Translation3d(c.pos) *
+			                    Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), correction));
 			rviz_marker_tools::makeArrow(m, depth, true);
 			result.markers().push_back(m);
 			if (failure)
 				break;
 
 			// fix collision by shifting object along correction direction
 			if (!dir.empty())  // if explicitly given, use this correction direction
-				tf::vectorMsgToEigen(boost::any_cast<geometry_msgs::Vector3>(dir), correction);
+				tf2::fromMsg(boost::any_cast<geometry_msgs::Vector3>(dir), correction);
 
 			const std::string& name = c.body_type_1 == cd::BodyTypes::WORLD_OBJECT ? c.body_name_1 : c.body_name_2;
 			scene.getWorldNonConst()->moveObject(name, Eigen::Isometry3d(Eigen::Translation3d(correction)));
diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp
@@ -43,7 +43,7 @@
 #include <moveit/robot_state/conversions.h>
 
 #include <Eigen/Geometry>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -168,7 +168,7 @@ void GenerateGraspPose::compute() {
 		current_angle += props.get<double>("angle_delta");
 
 		InterfaceState state(scene);
-		tf::poseEigenToMsg(target_pose, target_pose_msg.pose);
+		target_pose_msg.pose = tf2::toMsg(target_pose);
 		state.properties().set("target_pose", target_pose_msg);
 		props.exposeTo(state.properties(), { "pregrasp", "grasp" });
 
diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp
@@ -45,7 +45,7 @@
 #include <moveit/robot_state/attached_body.h>
 
 #include <Eigen/Geometry>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -97,13 +97,13 @@ void GeneratePlacePose::compute() {
 
 	const geometry_msgs::PoseStamped& pose_msg = props.get<geometry_msgs::PoseStamped>("pose");
 	Eigen::Isometry3d target_pose;
-	tf::poseMsgToEigen(pose_msg.pose, target_pose);
+	tf2::fromMsg(pose_msg.pose, target_pose);
 	// target pose w.r.t. planning frame
 	scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose);
 
 	const geometry_msgs::PoseStamped& ik_frame_msg = props.get<geometry_msgs::PoseStamped>("ik_frame");
 	Eigen::Isometry3d ik_frame;
-	tf::poseMsgToEigen(ik_frame_msg.pose, ik_frame);
+	tf2::fromMsg(ik_frame_msg.pose, ik_frame);
 	ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame;
 	Eigen::Isometry3d object_to_ik = orig_object_pose.inverse() * ik_frame;
 
@@ -123,7 +123,7 @@ void GeneratePlacePose::compute() {
 				// target ik_frame's pose w.r.t. planning frame
 				geometry_msgs::PoseStamped target_pose_msg;
 				target_pose_msg.header.frame_id = scene->getPlanningFrame();
-				tf::poseEigenToMsg(object * object_to_ik, target_pose_msg.pose);
+				target_pose_msg.pose = tf2::toMsg(object * object_to_ik);
 
 				InterfaceState state(scene);
 				forwardProperties(*s.end(), state);  // forward properties from inner solutions
diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp
@@ -41,7 +41,7 @@
 
 #include <moveit/planning_scene/planning_scene.h>
 #include <rviz_marker_tools/marker_creation.h>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -70,7 +70,7 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
 void MoveRelative::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
 	geometry_msgs::PoseStamped pose_msg;
 	pose_msg.header.frame_id = link;
-	tf::poseEigenToMsg(pose, pose_msg.pose);
+	pose_msg.pose = tf2::toMsg(pose);
 	setIKFrame(pose_msg);
 }
 
@@ -139,8 +139,8 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
 			rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance);
 			rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
 			// position half-way between pos_link and pos_reached
-			tf::pointEigenToMsg(0.5 * (pos_link + pos_reached), m.pose.position);
-			tf::quaternionEigenToMsg(quat_cylinder, m.pose.orientation);
+			m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) });
+			m.pose.orientation = tf2::toMsg(quat_cylinder);
 			markers.push_back(m);
 		}
 	} else {
@@ -154,8 +154,8 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
 			rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, linear_norm - distance);
 			rviz_marker_tools::setColor(m.color, rviz_marker_tools::RED);
 			// position half-way between pos_reached and pos_target
-			tf::pointEigenToMsg(0.5 * (pos_reached + pos_target), m.pose.position);
-			tf::quaternionEigenToMsg(quat_cylinder, m.pose.orientation);
+			m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_reached + pos_target) });
+			m.pose.orientation = tf2::toMsg(quat_cylinder);
 			markers.push_back(m);
 		}
 	}
@@ -224,8 +224,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 		try {  // try to extract Twist
 			const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(direction);
 			const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
-			tf::vectorMsgToEigen(target.twist.linear, linear);
-			tf::vectorMsgToEigen(target.twist.angular, angular);
+			tf2::fromMsg(target.twist.linear, linear);
+			tf2::fromMsg(target.twist.angular, angular);
 
 			linear_norm = linear.norm();
 			angular_norm = angular.norm();
@@ -267,7 +267,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 		try {  // try to extract Vector
 			const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(direction);
 			const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
-			tf::vectorMsgToEigen(target.vector, linear);
+			tf2::fromMsg(target.vector, linear);
 
 			// use max distance?
 			if (max_distance > 0.0) {
@@ -292,7 +292,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 	COMPUTE:
 		// transform target pose such that ik frame will reach there if link does
 		Eigen::Isometry3d ik_pose;
-		tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
+		tf2::fromMsg(ik_pose_msg.pose, ik_pose);
 		target_eigen = target_eigen * ik_pose.inverse();
 
 		success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp
@@ -36,13 +36,13 @@
    Desc:    Move to joint-state or Cartesian goal pose
 */
 
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_state/conversions.h>
+#include <tf2_eigen/tf2_eigen.h>
+
 #include <moveit/task_constructor/stages/move_to.h>
 #include <moveit/task_constructor/cost_terms.h>
-
-#include <moveit/planning_scene/planning_scene.h>
 #include <rviz_marker_tools/marker_creation.h>
-#include <eigen_conversions/eigen_msg.h>
-#include <moveit/robot_state/conversions.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -70,7 +70,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
 void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
 	geometry_msgs::PoseStamped pose_msg;
 	pose_msg.header.frame_id = link;
-	tf::poseEigenToMsg(pose, pose_msg.pose);
+	pose_msg.pose = tf2::toMsg(pose);
 	setIKFrame(pose_msg);
 }
 
@@ -145,7 +145,7 @@ bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningS
                          Eigen::Isometry3d& target_eigen) {
 	try {
 		const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal);
-		tf::poseMsgToEigen(target.pose, target_eigen);
+		tf2::fromMsg(target.pose, target_eigen);
 
 		// transform target into global frame
 		const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
@@ -161,7 +161,7 @@ bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel*
 	try {
 		const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
 		Eigen::Vector3d target_point;
-		tf::pointMsgToEigen(target.point, target_point);
+		tf2::fromMsg(target.point, target_point);
 
 		// transform target into global frame
 		const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
@@ -234,13 +234,13 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 		// visualize plan with frame at target pose and frame at link
 		geometry_msgs::PoseStamped target;
 		target.header.frame_id = scene->getPlanningFrame();
-		tf::poseEigenToMsg(target_eigen, target.pose);
+		target.pose = tf2::toMsg(target_eigen);
 		rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame");
 		rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
 
 		// transform target pose such that ik frame will reach there if link does
 		Eigen::Isometry3d ik_pose;
-		tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
+		tf2::fromMsg(ik_pose_msg.pose, ik_pose);
 		target_eigen = target_eigen * ik_pose.inverse();
 
 		// plan to Cartesian target
diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp
@@ -44,7 +44,7 @@
 #include <moveit/planning_scene/planning_scene.h>
 
 #include <Eigen/Geometry>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -145,7 +145,7 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model)
 void SimpleGraspBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
 	geometry_msgs::PoseStamped pose_msg;
 	pose_msg.header.frame_id = link;
-	tf::poseEigenToMsg(pose, pose_msg.pose);
+	pose_msg.pose = tf2::toMsg(pose);
 	setIKFrame(pose_msg);
 }
 
diff --git a/rviz_marker_tools/src/marker_creation.cpp b/rviz_marker_tools/src/marker_creation.cpp
@@ -1,6 +1,6 @@
 #include <rviz_marker_tools/marker_creation.h>
 #include <urdf_model/link.h>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 #include <ros/console.h>
 
 namespace vm = visualization_msgs;
@@ -139,21 +139,17 @@ std_msgs::ColorRGBA getColor(Color color, double alpha) {
 }
 
 geometry_msgs::Pose composePoses(const geometry_msgs::Pose& first, const Eigen::Isometry3d& second) {
-	geometry_msgs::Pose result;
 	Eigen::Isometry3d result_eigen;
-	tf::poseMsgToEigen(first, result_eigen);
+	tf2::fromMsg(first, result_eigen);
 	result_eigen = result_eigen * second;
-	tf::poseEigenToMsg(result_eigen, result);
-	return result;
+	return tf2::toMsg(result_eigen);
 }
 
 geometry_msgs::Pose composePoses(const Eigen::Isometry3d& first, const geometry_msgs::Pose& second) {
-	geometry_msgs::Pose result;
 	Eigen::Isometry3d result_eigen;
-	tf::poseMsgToEigen(second, result_eigen);
+	tf2::fromMsg(second, result_eigen);
 	result_eigen = first * result_eigen;
-	tf::poseEigenToMsg(result_eigen, result);
-	return result;
+	return tf2::toMsg(result_eigen);
 }
 
 void prepareMarker(vm::Marker& m, int marker_type) {
@@ -285,8 +281,8 @@ vm::Marker& makeArrow(vm::Marker& m, const Eigen::Vector3d& start_point, const E
 	m.scale.z = head_length;
 	prepareMarker(m, vm::Marker::ARROW);
 	m.points.resize(2);
-	tf::pointEigenToMsg(start_point, m.points[0]);
-	tf::pointEigenToMsg(end_point, m.points[1]);
+	m.points[0] = tf2::toMsg(start_point);
+	m.points[1] = tf2::toMsg(end_point);
 	return m;
 }
 
